BRSRI/bellande_humanoid_robot_intelligence_control_system_framework
1
0
Fork 0
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

Merge pull request #2 from RonaldsonBellande/noetic

Browse Source 
Travis
This commit is contained in:
Ronaldson Bellande 2022-05-13 14:28:57 -04:00 committed by GitHub
commit a682599e77
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 422 additions and 449 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.robotis_framework.rosinstall
View File

@ -1 +1 @@
- git: {local-name: robotis_controller_msgs, uri: 'https://github.com/ROBOTIS-GIT/ROBOTIS-Framework-msgs.git', version: master}
- git: {local-name: robotis_controller_msgs, uri: 'https://github.com/Robotics-Sensors/ROBOTIS-Framework-msgs.git', version: noetic}

11
.travis.yml
View File

@ -7,7 +7,7 @@ services:
- docker
language: generic
python:
- "2.7"
- "3.8"
compiler:
- gcc
notifications:
@ -15,17 +15,14 @@ notifications:
on_success: change
on_failure: always
recipients:
- pyo@robotis.com
- ronaldsonbellande@gmail.com
env:
matrix:
# - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian
# - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=jessie
- ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=xenial $ROSINSTALL_FILENAME=".robotis_framework.rosinstall"
- ROS_DISTRO=noetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=focal $ROSINSTALL_FILENAME=".robotis_framework.rosinstall"
branches:
only:
- master
- develop
- kinetic-devel
- noetic
install:
- git clone https://github.com/ros-industrial/industrial_ci.git .ci_config
script:

265
robotis_controller/include/robotis_controller/robotis_controller.h
View File

@ -1,18 +1,18 @@
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/*
* robotis_controller.h
@ -22,96 +22,99 @@
*/
#ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_
#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_
#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_
#include <boost/thread.hpp>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float64.h>
#include <std_msgs/String.h>
#include <yaml-cpp/yaml.h>
#include "robotis_controller_msgs/GetJointModule.h"
#include "robotis_controller_msgs/JointCtrlModule.h"
#include "robotis_controller_msgs/LoadOffset.h"
#include "robotis_controller_msgs/SetJointModule.h"
#include "robotis_controller_msgs/SetModule.h"
#include "robotis_controller_msgs/SyncWriteItem.h"
#include "robotis_controller_msgs/WriteControlTable.h"
#include <ros/ros.h>
#include <boost/thread.hpp>
#include <yaml-cpp/yaml.h>
#include <std_msgs/Bool.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>
#include <sensor_msgs/JointState.h>
#include "dynamixel_sdk/group_bulk_read.h"
#include "dynamixel_sdk/group_sync_write.h"
#include "robotis_device/robot.h"
#include "robotis_framework_common/motion_module.h"
#include "robotis_framework_common/sensor_module.h"
#include "robotis_controller_msgs/WriteControlTable.h"
#include "robotis_controller_msgs/SyncWriteItem.h"
#include "robotis_controller_msgs/JointCtrlModule.h"
#include "robotis_controller_msgs/GetJointModule.h"
#include "robotis_controller_msgs/SetJointModule.h"
#include "robotis_controller_msgs/SetModule.h"
#include "robotis_controller_msgs/LoadOffset.h"
namespace robotis_framework {
#include "robotis_device/robot.h"
#include "robotis_framework_common/motion_module.h"
#include "robotis_framework_common/sensor_module.h"
#include "dynamixel_sdk/group_bulk_read.h"
#include "dynamixel_sdk/group_sync_write.h"
enum ControllerMode { MotionModuleMode, DirectControlMode };
namespace robotis_framework
{
enum ControllerMode
{
MotionModuleMode,
DirectControlMode
};
class RobotisController : public Singleton<RobotisController>
{
class RobotisController : public Singleton<RobotisController> {
private:
boost::thread queue_thread_;
boost::thread gazebo_thread_;
boost::thread set_module_thread_;
boost::mutex queue_mutex_;
boost::thread queue_thread_;
boost::thread gazebo_thread_;
boost::thread set_module_thread_;
boost::mutex queue_mutex_;
bool init_pose_loaded_;
bool is_timer_running_;
bool is_offset_enabled_;
double offset_ratio_;
bool stop_timer_;
pthread_t timer_thread_;
ControllerMode controller_mode_;
bool init_pose_loaded_;
bool is_timer_running_;
bool is_offset_enabled_;
double offset_ratio_;
bool stop_timer_;
pthread_t timer_thread_;
ControllerMode controller_mode_;
std::list<MotionModule *> motion_modules_;
std::list<SensorModule *> sensor_modules_;
std::vector<dynamixel::GroupSyncWrite *> direct_sync_write_;
std::vector<dynamixel::GroupSyncWrite *> direct_sync_write_;
std::map<std::string, double> sensor_result_;
void gazeboTimerThread();
void msgQueueThread();
void setCtrlModuleThread(std::string ctrl_module);
void setJointCtrlModuleThread(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg);
void setJointCtrlModuleThread(
const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg);
bool isTimerStopped();
void initializeSyncWrite();
public:
const int NONE_GAIN = 65535;
bool DEBUG_PRINT;
Robot *robot_;
const int NONE_GAIN = 65535;
bool DEBUG_PRINT;
Robot *robot_;
bool gazebo_mode_;
std::string gazebo_robot_name_;
bool gazebo_mode_;
std::string gazebo_robot_name_;
/* bulk read */
std::map<std::string, dynamixel::GroupBulkRead *> port_to_bulk_read_;
std::map<std::string, dynamixel::GroupBulkRead *> port_to_bulk_read_;
/* sync write */
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_position_;
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_velocity_;
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_current_;
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_position_p_gain_;
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_position_i_gain_;
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_position_d_gain_;
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_velocity_p_gain_;
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_velocity_i_gain_;
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_velocity_d_gain_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_position_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_velocity_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_current_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_position_p_gain_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_position_i_gain_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_position_d_gain_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_velocity_p_gain_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_velocity_i_gain_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_velocity_d_gain_;
/* publisher */
ros::Publisher goal_joint_state_pub_;
ros::Publisher present_joint_state_pub_;
ros::Publisher current_module_pub_;
ros::Publisher goal_joint_state_pub_;
ros::Publisher present_joint_state_pub_;
ros::Publisher current_module_pub_;
std::map<std::string, ros::Publisher> gazebo_joint_position_pub_;
std::map<std::string, ros::Publisher> gazebo_joint_velocity_pub_;
@ -121,62 +124,84 @@ public:
RobotisController();
bool initialize(const std::string robot_file_path, const std::string init_file_path);
void initializeDevice(const std::string init_file_path);
void process();
bool initialize(const std::string robot_file_path,
const std::string init_file_path);
void initializeDevice(const std::string init_file_path);
void process();
void addMotionModule(MotionModule *module);
void removeMotionModule(MotionModule *module);
void addSensorModule(SensorModule *module);
void removeSensorModule(SensorModule *module);
void addMotionModule(MotionModule *module);
void removeMotionModule(MotionModule *module);
void addSensorModule(SensorModule *module);
void removeSensorModule(SensorModule *module);
void startTimer();
void stopTimer();
bool isTimerRunning();
void startTimer();
void stopTimer();
bool isTimerRunning();
void setCtrlModule(std::string module_name);
void loadOffset(const std::string path);
void setCtrlModule(std::string module_name);
void loadOffset(const std::string path);
/* ROS Topic Callback Functions */
void writeControlTableCallback(const robotis_controller_msgs::WriteControlTable::ConstPtr &msg);
void syncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg);
void setControllerModeCallback(const std_msgs::String::ConstPtr &msg);
void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
void setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg);
void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg);
void enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg);
bool getJointCtrlModuleService(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res);
bool setJointCtrlModuleService(robotis_controller_msgs::SetJointModule::Request &req, robotis_controller_msgs::SetJointModule::Response &res);
bool setCtrlModuleService(robotis_controller_msgs::SetModule::Request &req, robotis_controller_msgs::SetModule::Response &res);
bool loadOffsetService(robotis_controller_msgs::LoadOffset::Request &req, robotis_controller_msgs::LoadOffset::Response &res);
void writeControlTableCallback(
const robotis_controller_msgs::WriteControlTable::ConstPtr &msg);
void syncWriteItemCallback(
const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg);
void setControllerModeCallback(const std_msgs::String::ConstPtr &msg);
void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
void setJointCtrlModuleCallback(
const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg);
void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg);
void enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg);
bool getJointCtrlModuleService(
robotis_controller_msgs::GetJointModule::Request &req,
robotis_controller_msgs::GetJointModule::Response &res);
bool setJointCtrlModuleService(
robotis_controller_msgs::SetJointModule::Request &req,
robotis_controller_msgs::SetJointModule::Response &res);
bool setCtrlModuleService(robotis_controller_msgs::SetModule::Request &req,
robotis_controller_msgs::SetModule::Response &res);
bool loadOffsetService(robotis_controller_msgs::LoadOffset::Request &req,
robotis_controller_msgs::LoadOffset::Response &res);
void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
int ping (const std::string joint_name, uint8_t *error = 0);
int ping (const std::string joint_name, uint16_t* model_number, uint8_t *error = 0);
int ping(const std::string joint_name, uint8_t *error = 0);
int ping(const std::string joint_name, uint16_t *model_number,
uint8_t *error = 0);
int action (const std::string joint_name);
int reboot (const std::string joint_name, uint8_t *error = 0);
int factoryReset(const std::string joint_name, uint8_t option = 0, uint8_t *error = 0);
int action(const std::string joint_name);
int reboot(const std::string joint_name, uint8_t *error = 0);
int factoryReset(const std::string joint_name, uint8_t option = 0,
uint8_t *error = 0);
int read (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0);
int readCtrlItem(const std::string joint_name, const std::string item_name, uint32_t *data, uint8_t *error = 0);
int read(const std::string joint_name, uint16_t address, uint16_t length,
uint8_t *data, uint8_t *error = 0);
int readCtrlItem(const std::string joint_name, const std::string item_name,
uint32_t *data, uint8_t *error = 0);
int read1Byte (const std::string joint_name, uint16_t address, uint8_t *data, uint8_t *error = 0);
int read2Byte (const std::string joint_name, uint16_t address, uint16_t *data, uint8_t *error = 0);
int read4Byte (const std::string joint_name, uint16_t address, uint32_t *data, uint8_t *error = 0);
int read1Byte(const std::string joint_name, uint16_t address, uint8_t *data,
uint8_t *error = 0);
int read2Byte(const std::string joint_name, uint16_t address, uint16_t *data,
uint8_t *error = 0);
int read4Byte(const std::string joint_name, uint16_t address, uint32_t *data,
uint8_t *error = 0);
int write (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0);
int writeCtrlItem(const std::string joint_name, const std::string item_name, uint32_t data, uint8_t *error = 0);
int write(const std::string joint_name, uint16_t address, uint16_t length,
uint8_t *data, uint8_t *error = 0);
int writeCtrlItem(const std::string joint_name, const std::string item_name,
uint32_t data, uint8_t *error = 0);
int write1Byte (const std::string joint_name, uint16_t address, uint8_t data, uint8_t *error = 0);
int write2Byte (const std::string joint_name, uint16_t address, uint16_t data, uint8_t *error = 0);
int write4Byte (const std::string joint_name, uint16_t address, uint32_t data, uint8_t *error = 0);
int write1Byte(const std::string joint_name, uint16_t address, uint8_t data,
uint8_t *error = 0);
int write2Byte(const std::string joint_name, uint16_t address, uint16_t data,
uint8_t *error = 0);
int write4Byte(const std::string joint_name, uint16_t address, uint32_t data,
uint8_t *error = 0);
int regWrite (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0);
int regWrite(const std::string joint_name, uint16_t address, uint16_t length,
uint8_t *data, uint8_t *error = 0);
};
}
} // namespace robotis_framework
#endif /* ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ */

84
robotis_device/include/robotis_device/control_table_item.h
View File

@ -1,18 +1,18 @@
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/*
* control_table_item.h
@ -22,49 +22,33 @@
*/
#ifndef ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_
#define ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_
#define ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_
#include <stdint.h>
namespace robotis_framework
{
namespace robotis_framework {
enum AccessType {
Read,
ReadWrite
};
enum AccessType { Read, ReadWrite };
enum MemoryType {
EEPROM,
RAM
};
enum MemoryType { EEPROM, RAM };
class ControlTableItem
{
class ControlTableItem {
public:
std::string item_name_;
uint16_t address_;
AccessType access_type_;
MemoryType memory_type_;
uint8_t data_length_;
int32_t data_min_value_;
int32_t data_max_value_;
bool is_signed_;
uint16_t address_;
AccessType access_type_;
MemoryType memory_type_;
uint8_t data_length_;
int32_t data_min_value_;
int32_t data_max_value_;
bool is_signed_;
ControlTableItem()
: item_name_(""),
address_(0),
access_type_(Read),
memory_type_(RAM),
data_length_(0),
data_min_value_(0),
data_max_value_(0),
is_signed_(false)
{ }
ControlTableItem()
: item_name_(""), address_(0), access_type_(Read), memory_type_(RAM),
data_length_(0), data_min_value_(0), data_max_value_(0),
is_signed_(false) {}
};
}
} // namespace robotis_framework
#endif /* ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ */

141
robotis_device/src/robotis_device/dynamixel.cpp
View File

@ -1,18 +1,18 @@
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/*
* dynamixel.cpp
@ -26,28 +26,15 @@
using namespace robotis_framework;
Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version)
: ctrl_module_name_("none"),
torque_to_current_value_ratio_(1.0),
velocity_to_value_ratio_(1.0),
value_of_0_radian_position_(0),
value_of_min_radian_position_(0),
value_of_max_radian_position_(0),
min_radian_(-3.14159265),
max_radian_(3.14159265),
torque_enable_item_(0),
present_position_item_(0),
present_velocity_item_(0),
present_current_item_(0),
goal_position_item_(0),
goal_velocity_item_(0),
goal_current_item_(0),
position_p_gain_item_(0),
position_i_gain_item_(0),
position_d_gain_item_(0),
velocity_p_gain_item_(0),
velocity_i_gain_item_(0),
velocity_d_gain_item_(0)
{
: ctrl_module_name_("none"), torque_to_current_value_ratio_(1.0),
velocity_to_value_ratio_(1.0), value_of_0_radian_position_(0),
value_of_min_radian_position_(0), value_of_max_radian_position_(0),
min_radian_(-3.14159265), max_radian_(3.14159265), torque_enable_item_(0),
present_position_item_(0), present_velocity_item_(0),
present_current_item_(0), goal_position_item_(0), goal_velocity_item_(0),
goal_current_item_(0), position_p_gain_item_(0), position_i_gain_item_(0),
position_d_gain_item_(0), velocity_p_gain_item_(0),
velocity_i_gain_item_(0), velocity_d_gain_item_(0) {
this->id_ = id;
this->model_name_ = model_name;
this->port_name_ = "";
@ -59,80 +46,74 @@ Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version)
bulk_read_items_.clear();
}
double Dynamixel::convertValue2Radian(int32_t value)
{
double Dynamixel::convertValue2Radian(int32_t value) {
double radian = 0.0;
if (value > value_of_0_radian_position_)
{
if (value > value_of_0_radian_position_) {
if (max_radian_ <= 0)
return max_radian_;
radian = (double) (value - value_of_0_radian_position_) * max_radian_
/ (double) (value_of_max_radian_position_ - value_of_0_radian_position_);
}
else if (value < value_of_0_radian_position_)
{
radian =
(double)(value - value_of_0_radian_position_) * max_radian_ /
(double)(value_of_max_radian_position_ - value_of_0_radian_position_);
} else if (value < value_of_0_radian_position_) {
if (min_radian_ >= 0)
return min_radian_;
radian = (double) (value - value_of_0_radian_position_) * min_radian_
/ (double) (value_of_min_radian_position_ - value_of_0_radian_position_);
radian =
(double)(value - value_of_0_radian_position_) * min_radian_ /
(double)(value_of_min_radian_position_ - value_of_0_radian_position_);
}
// if (radian > max_radian_)
// return max_radian_;
// else if (radian < min_radian_)
// return min_radian_;
// if (radian > max_radian_)
// return max_radian_;
// else if (radian < min_radian_)
// return min_radian_;
return radian;
}
int32_t Dynamixel::convertRadian2Value(double radian)
{
int32_t Dynamixel::convertRadian2Value(double radian) {
int32_t value = 0;
if (radian > 0)
{
if (radian > 0) {
if (value_of_max_radian_position_ <= value_of_0_radian_position_)
return value_of_max_radian_position_;
value = (radian * (value_of_max_radian_position_ - value_of_0_radian_position_) / max_radian_)
+ value_of_0_radian_position_;
}
else if (radian < 0)
{
value = (radian *
(value_of_max_radian_position_ - value_of_0_radian_position_) /
max_radian_) +
value_of_0_radian_position_;
} else if (radian < 0) {
if (value_of_min_radian_position_ >= value_of_0_radian_position_)
return value_of_min_radian_position_;
value = (radian * (value_of_min_radian_position_ - value_of_0_radian_position_) / min_radian_)
+ value_of_0_radian_position_;
}
else
value = (radian *
(value_of_min_radian_position_ - value_of_0_radian_position_) /
min_radian_) +
value_of_0_radian_position_;
} else
value = value_of_0_radian_position_;
// if (value > value_of_max_radian_position_)
// return value_of_max_radian_position_;
// else if (value < value_of_min_radian_position_)
// return value_of_min_radian_position_;
// if (value > value_of_max_radian_position_)
// return value_of_max_radian_position_;
// else if (value < value_of_min_radian_position_)
// return value_of_min_radian_position_;
return value;
}
double Dynamixel::convertValue2Velocity(int32_t value)
{
return (double) value / velocity_to_value_ratio_;
double Dynamixel::convertValue2Velocity(int32_t value) {
return (double)value / velocity_to_value_ratio_;
}
int32_t Dynamixel::convertVelocity2Value(double velocity)
{
return (int32_t) (velocity * velocity_to_value_ratio_);;
int32_t Dynamixel::convertVelocity2Value(double velocity) {
return (int32_t)(velocity * velocity_to_value_ratio_);
;
}
double Dynamixel::convertValue2Torque(int16_t value)
{
return (double) value / torque_to_current_value_ratio_;
double Dynamixel::convertValue2Torque(int16_t value) {
return (double)value / torque_to_current_value_ratio_;
}
int16_t Dynamixel::convertTorque2Value(double torque)
{
return (int16_t) (torque * torque_to_current_value_ratio_);
int16_t Dynamixel::convertTorque2Value(double torque) {
return (int16_t)(torque * torque_to_current_value_ratio_);
}

316
robotis_device/src/robotis_device/robot.cpp
View File

@ -1,18 +1,18 @@
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/*
* robot.cpp
@ -21,36 +21,35 @@
* Author: zerom
*/
#include <fstream>
#include <iostream>
#include <algorithm>
#include <algorithm>
#include <fstream>
#include <iostream>
#include "robotis_device/robot.h"
using namespace robotis_framework;
static inline std::string &ltrim(std::string &s)
{
s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun<int, int>(std::isspace))));
static inline std::string &ltrim(std::string &s) {
s.erase(s.begin(),
std::find_if(s.begin(), s.end(),
std::not1(std::ptr_fun<int, int>(std::isspace))));
return s;
}
static inline std::string &rtrim(std::string &s)
{
s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun<int, int>(std::isspace))).base(), s.end());
static inline std::string &rtrim(std::string &s) {
s.erase(std::find_if(s.rbegin(), s.rend(),
std::not1(std::ptr_fun<int, int>(std::isspace)))
.base(),
s.end());
return s;
}
static inline std::string &trim(std::string &s)
{
return ltrim(rtrim(s));
}
static inline std::string &trim(std::string &s) { return ltrim(rtrim(s)); }
static inline std::vector<std::string> split(const std::string &text, char sep)
{
static inline std::vector<std::string> split(const std::string &text,
char sep) {
std::vector<std::string> tokens;
std::size_t start = 0, end = 0;
while ((end = text.find(sep, start)) != (std::string::npos))
{
while ((end = text.find(sep, start)) != (std::string::npos)) {
tokens.push_back(text.substr(start, end - start));
trim(tokens.back());
start = end + 1;
@ -61,19 +60,16 @@ static inline std::vector<std::string> split(const std::string &text, char sep)
return tokens;
}
Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
: control_cycle_msec_(DEFAULT_CONTROL_CYCLE)
{
Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
: control_cycle_msec_(DEFAULT_CONTROL_CYCLE) {
if (dev_desc_dir_path.compare(dev_desc_dir_path.size() - 1, 1, "/") != 0)
dev_desc_dir_path += "/";
std::ifstream file(robot_file_path.c_str());
if (file.is_open())
{
if (file.is_open()) {
std::string session = "";
std::string input_str;
while (!file.eof())
{
while (!file.eof()) {
std::getline(file, input_str);
// remove comment ( # )
@ -85,160 +81,157 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
input_str = trim(input_str);
// find session
if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]"))
{
if (!input_str.compare(0, 1, "[") &&
!input_str.compare(input_str.size() - 1, 1, "]")) {
input_str = input_str.substr(1, input_str.size() - 2);
std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower);
std::transform(input_str.begin(), input_str.end(), input_str.begin(),
::tolower);
session = trim(input_str);
continue;
}
if (session == SESSION_CONTROL_INFO)
{
if (session == SESSION_CONTROL_INFO) {
std::vector<std::string> tokens = split(input_str, '=');
if (tokens.size() != 2)
continue;
if (tokens[0] == "control_cycle")
control_cycle_msec_ = std::atoi(tokens[1].c_str());
}
else if (session == SESSION_PORT_INFO)
{
} else if (session == SESSION_PORT_INFO) {
std::vector<std::string> tokens = split(input_str, '|');
if (tokens.size() != 3)
continue;
std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" << std::endl;
std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")"
<< std::endl;
ports_[tokens[0]] = dynamixel::PortHandler::getPortHandler(tokens[0].c_str());
ports_[tokens[0]] =
dynamixel::PortHandler::getPortHandler(tokens[0].c_str());
ports_[tokens[0]]->setBaudRate(std::atoi(tokens[1].c_str()));
port_default_device_[tokens[0]] = tokens[2];
}
else if (session == SESSION_DEVICE_INFO)
{
} else if (session == SESSION_DEVICE_INFO) {
std::vector<std::string> tokens = split(input_str, '|');
if (tokens.size() != 7)
continue;
if (tokens[0] == DYNAMIXEL)
{
std::string file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device";
int id = std::atoi(tokens[2].c_str());
std::string port = tokens[1];
float protocol = std::atof(tokens[4].c_str());
std::string dev_name = tokens[5];
if (tokens[0] == DYNAMIXEL) {
std::string file_path =
dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device";
int id = std::atoi(tokens[2].c_str());
std::string port = tokens[1];
float protocol = std::atof(tokens[4].c_str());
std::string dev_name = tokens[5];
dxls_[dev_name] = getDynamixel(file_path, id, port, protocol);
Dynamixel *dxl = dxls_[dev_name];
std::vector<std::string> sub_tokens = split(tokens[6], ',');
if (sub_tokens.size() > 0 && sub_tokens[0] != "")
{
std::map<std::string, ControlTableItem *>::iterator indirect_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1);
if (indirect_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist
if (sub_tokens.size() > 0 && sub_tokens[0] != "") {
std::map<std::string, ControlTableItem *>::iterator indirect_it =
dxl->ctrl_table_.find(INDIRECT_ADDRESS_1);
if (indirect_it !=
dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist
{
uint16_t indirect_data_addr = dxl->ctrl_table_[INDIRECT_DATA_1]->address_;
for (int _i = 0; _i < sub_tokens.size(); _i++)
{
std::map<std::string, ControlTableItem *>::iterator bulkread_it = dxl->ctrl_table_.find(sub_tokens[_i]);
if(bulkread_it == dxl->ctrl_table_.end())
{
fprintf(stderr, "\n ##### BULK READ ITEM [ %s ] NOT FOUND!! #####\n\n", sub_tokens[_i].c_str());
uint16_t indirect_data_addr =
dxl->ctrl_table_[INDIRECT_DATA_1]->address_;
for (int _i = 0; _i < sub_tokens.size(); _i++) {
std::map<std::string, ControlTableItem *>::iterator
bulkread_it = dxl->ctrl_table_.find(sub_tokens[_i]);
if (bulkread_it == dxl->ctrl_table_.end()) {
fprintf(
stderr,
"\n ##### BULK READ ITEM [ %s ] NOT FOUND!! #####\n\n",
sub_tokens[_i].c_str());
continue;
}
dxl->bulk_read_items_.push_back(new ControlTableItem());
ControlTableItem *dest_item = dxl->bulk_read_items_[_i];
ControlTableItem *src_item = dxl->ctrl_table_[sub_tokens[_i]];
ControlTableItem *src_item = dxl->ctrl_table_[sub_tokens[_i]];
dest_item->item_name_ = src_item->item_name_;
dest_item->address_ = indirect_data_addr;
dest_item->access_type_ = src_item->access_type_;
dest_item->memory_type_ = src_item->memory_type_;
dest_item->data_length_ = src_item->data_length_;
dest_item->data_min_value_ = src_item->data_min_value_;
dest_item->data_max_value_ = src_item->data_max_value_;
dest_item->is_signed_ = src_item->is_signed_;
dest_item->item_name_ = src_item->item_name_;
dest_item->address_ = indirect_data_addr;
dest_item->access_type_ = src_item->access_type_;
dest_item->memory_type_ = src_item->memory_type_;
dest_item->data_length_ = src_item->data_length_;
dest_item->data_min_value_ = src_item->data_min_value_;
dest_item->data_max_value_ = src_item->data_max_value_;
dest_item->is_signed_ = src_item->is_signed_;
indirect_data_addr += dest_item->data_length_;
}
}
else // INDIRECT_ADDRESS_1 not exist
} else // INDIRECT_ADDRESS_1 not exist
{
for (int i = 0; i < sub_tokens.size(); i++)
{
for (int i = 0; i < sub_tokens.size(); i++) {
if (dxl->ctrl_table_[sub_tokens[i]] != NULL)
dxl->bulk_read_items_.push_back(dxl->ctrl_table_[sub_tokens[i]]);
dxl->bulk_read_items_.push_back(
dxl->ctrl_table_[sub_tokens[i]]);
}
}
}
}
else if (tokens[0] == SENSOR)
{
std::string file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device";
int id = std::atoi(tokens[2].c_str());
std::string port = tokens[1];
float protocol = std::atof(tokens[4].c_str());
std::string dev_name = tokens[5];
} else if (tokens[0] == SENSOR) {
std::string file_path =
dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device";
int id = std::atoi(tokens[2].c_str());
std::string port = tokens[1];
float protocol = std::atof(tokens[4].c_str());
std::string dev_name = tokens[5];
sensors_[dev_name] = getSensor(file_path, id, port, protocol);
Sensor *sensor = sensors_[dev_name];
std::vector<std::string> sub_tokens = split(tokens[6], ',');
if (sub_tokens.size() > 0 && sub_tokens[0] != "")
{
std::map<std::string, ControlTableItem *>::iterator indirect_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1);
if (indirect_it != sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist
if (sub_tokens.size() > 0 && sub_tokens[0] != "") {
std::map<std::string, ControlTableItem *>::iterator indirect_it =
sensor->ctrl_table_.find(INDIRECT_ADDRESS_1);
if (indirect_it !=
sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist
{
uint16_t indirect_data_addr = sensor->ctrl_table_[INDIRECT_DATA_1]->address_;
for (int i = 0; i < sub_tokens.size(); i++)
{
uint16_t indirect_data_addr =
sensor->ctrl_table_[INDIRECT_DATA_1]->address_;
for (int i = 0; i < sub_tokens.size(); i++) {
sensor->bulk_read_items_.push_back(new ControlTableItem());
ControlTableItem *dest_item = sensor->bulk_read_items_[i];
ControlTableItem *src_item = sensor->ctrl_table_[sub_tokens[i]];
ControlTableItem *src_item = sensor->ctrl_table_[sub_tokens[i]];
dest_item->item_name_ = src_item->item_name_;
dest_item->address_ = indirect_data_addr;
dest_item->access_type_ = src_item->access_type_;
dest_item->memory_type_ = src_item->memory_type_;
dest_item->data_length_ = src_item->data_length_;
dest_item->data_min_value_ = src_item->data_min_value_;
dest_item->data_max_value_ = src_item->data_max_value_;
dest_item->is_signed_ = src_item->is_signed_;
dest_item->item_name_ = src_item->item_name_;
dest_item->address_ = indirect_data_addr;
dest_item->access_type_ = src_item->access_type_;
dest_item->memory_type_ = src_item->memory_type_;
dest_item->data_length_ = src_item->data_length_;
dest_item->data_min_value_ = src_item->data_min_value_;
dest_item->data_max_value_ = src_item->data_max_value_;
dest_item->is_signed_ = src_item->is_signed_;
indirect_data_addr += dest_item->data_length_;
}
}
else // INDIRECT_ADDRESS_1 exist
} else // INDIRECT_ADDRESS_1 exist
{
for (int i = 0; i < sub_tokens.size(); i++)
sensor->bulk_read_items_.push_back(sensor->ctrl_table_[sub_tokens[i]]);
sensor->bulk_read_items_.push_back(
sensor->ctrl_table_[sub_tokens[i]]);
}
}
}
}
}
file.close();
}
else
{
} else {
std::cout << "Unable to open file : " + robot_file_path << std::endl;
}
}
Sensor *Robot::getSensor(std::string path, int id, std::string port, float protocol_version)
{
Sensor *Robot::getSensor(std::string path, int id, std::string port,
float protocol_version) {
Sensor *sensor;
// load file model_name.device
std::ifstream file(path.c_str());
if (file.is_open())
{
if (file.is_open()) {
std::string session = "";
std::string input_str;
while (!file.eof())
{
while (!file.eof()) {
std::getline(file, input_str);
// remove comment ( # )
@ -252,33 +245,31 @@ Sensor *Robot::getSensor(std::string path, int id, std::string port, float proto
continue;
// find _session
if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]"))
{
if (!input_str.compare(0, 1, "[") &&
!input_str.compare(input_str.size() - 1, 1, "]")) {
input_str = input_str.substr(1, input_str.size() - 2);
std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower);
std::transform(input_str.begin(), input_str.end(), input_str.begin(),
::tolower);
session = trim(input_str);
continue;
}
if (session == SESSION_DEVICE_INFO)
{
if (session == SESSION_DEVICE_INFO) {
std::vector<std::string> tokens = split(input_str, '=');
if (tokens.size() != 2)
continue;
if (tokens[0] == "model_name")
sensor = new Sensor(id, tokens[1], protocol_version);
}
else if (session == SESSION_CONTROL_TABLE)
{
} else if (session == SESSION_CONTROL_TABLE) {
std::vector<std::string> tokens = split(input_str, '|');
if (tokens.size() != 8)
continue;
ControlTableItem *item = new ControlTableItem();
item->item_name_ = tokens[1];
item->address_ = std::atoi(tokens[0].c_str());
item->data_length_ = std::atoi(tokens[2].c_str());
item->item_name_ = tokens[1];
item->address_ = std::atoi(tokens[0].c_str());
item->data_length_ = std::atoi(tokens[2].c_str());
if (tokens[3] == "R")
item->access_type_ = Read;
@ -302,24 +293,24 @@ Sensor *Robot::getSensor(std::string path, int id, std::string port, float proto
}
sensor->port_name_ = port;
fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), sensor->id_, sensor->model_name_.c_str());
//std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name << " added. (" << port << ")" << std::endl;
fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), sensor->id_,
sensor->model_name_.c_str());
// std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name
// << " added. (" << port << ")" << std::endl;
file.close();
}
else
} else
std::cout << "Unable to open file : " + path << std::endl;
return sensor;
}
Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float protocol_version)
{
Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port,
float protocol_version) {
Dynamixel *dxl;
// load file model_name.device
std::ifstream file(path.c_str());
if (file.is_open())
{
if (file.is_open()) {
std::string session = "";
std::string input_str;
@ -337,8 +328,7 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float
std::string velocity_i_gain_item_name = "";
std::string velocity_p_gain_item_name = "";
while (!file.eof())
{
while (!file.eof()) {
std::getline(file, input_str);
// remove comment ( # )
@ -352,25 +342,23 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float
continue;
// find session
if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]"))
{
if (!input_str.compare(0, 1, "[") &&
!input_str.compare(input_str.size() - 1, 1, "]")) {
input_str = input_str.substr(1, input_str.size() - 2);
std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower);
std::transform(input_str.begin(), input_str.end(), input_str.begin(),
::tolower);
session = trim(input_str);
continue;
}
if (session == SESSION_DEVICE_INFO)
{
if (session == SESSION_DEVICE_INFO) {
std::vector<std::string> tokens = split(input_str, '=');
if (tokens.size() != 2)
continue;
if (tokens[0] == "model_name")
dxl = new Dynamixel(id, tokens[1], protocol_version);
}
else if (session == SESSION_TYPE_INFO)
{
} else if (session == SESSION_TYPE_INFO) {
std::vector<std::string> tokens = split(input_str, '=');
if (tokens.size() != 2)
continue;
@ -417,17 +405,15 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float
velocity_i_gain_item_name = tokens[1];
else if (tokens[0] == "velocity_p_gain_item_name")
velocity_p_gain_item_name = tokens[1];
}
else if (session == SESSION_CONTROL_TABLE)
{
} else if (session == SESSION_CONTROL_TABLE) {
std::vector<std::string> tokens = split(input_str, '|');
if (tokens.size() != 8)
continue;
ControlTableItem *item = new ControlTableItem();
item->item_name_ = tokens[1];
item->address_ = std::atoi(tokens[0].c_str());
item->data_length_ = std::atoi(tokens[2].c_str());
item->item_name_ = tokens[1];
item->address_ = std::atoi(tokens[0].c_str());
item->data_length_ = std::atoi(tokens[2].c_str());
if (tokens[3] == "R")
item->access_type_ = Read;
@ -454,9 +440,11 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float
if (dxl->ctrl_table_[torque_enable_item_name] != NULL)
dxl->torque_enable_item_ = dxl->ctrl_table_[torque_enable_item_name];
if (dxl->ctrl_table_[present_position_item_name] != NULL)
dxl->present_position_item_ = dxl->ctrl_table_[present_position_item_name];
dxl->present_position_item_ =
dxl->ctrl_table_[present_position_item_name];
if (dxl->ctrl_table_[present_velocity_item_name] != NULL)
dxl->present_velocity_item_ = dxl->ctrl_table_[present_velocity_item_name];
dxl->present_velocity_item_ =
dxl->ctrl_table_[present_velocity_item_name];
if (dxl->ctrl_table_[present_current_item_name] != NULL)
dxl->present_current_item_ = dxl->ctrl_table_[present_current_item_name];
if (dxl->ctrl_table_[goal_position_item_name] != NULL)
@ -478,17 +466,15 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float
if (dxl->ctrl_table_[velocity_p_gain_item_name] != NULL)
dxl->velocity_p_gain_item_ = dxl->ctrl_table_[velocity_p_gain_item_name];
fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id_, dxl->model_name_.c_str());
//std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " added. (" << port << ")" << std::endl;
fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id_,
dxl->model_name_.c_str());
// std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << "
// added. (" << port << ")" << std::endl;
file.close();
}
else
} else
std::cout << "Unable to open file : " + path << std::endl;
return dxl;
}
int Robot::getControlCycle()
{
return control_cycle_msec_;
}
int Robot::getControlCycle() { return control_cycle_msec_; }

52
robotis_device/src/robotis_device/sensor.cpp
View File

@ -1,18 +1,18 @@
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/*
* sensor.cpp
@ -25,15 +25,15 @@
using namespace robotis_framework;
Sensor::Sensor(int id, std::string model_name, float protocol_version)
{
this->id_ = id;
this->model_name_ = model_name;
this->port_name_ = "";
this->protocol_version_ = protocol_version;
ctrl_table_.clear();
sensor_state_ = new SensorState();
bulk_read_items_.clear();
Sensor::Sensor(int id, std::string model_name, float protocol_version) {
this->id_ = id;
this->model_name_ = model_name;
this->port_name_ = "";
this->protocol_version_ = protocol_version;
ctrl_table_.clear();
sensor_state_ = new SensorState();
bulk_read_items_.clear();
}