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

- ROS C++ Coding Style is applied.

Browse Source
This commit is contained in:
ROBOTIS-zerom
2016-06-03 20:05:21 +09:00
parent 4dcaa5900f
commit 1ed888af00
50 changed files with 5772 additions and 4681 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
robotis_device/CMakeLists.txt
View File

@ -8,15 +8,6 @@ find_package(catkin REQUIRED COMPONENTS
robotis_framework_common
)
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
LIBRARIES robotis_device
@ -24,38 +15,19 @@ catkin_package(
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
add_library(robotis_device
src/robotis_device/Robot.cpp
src/robotis_device/Sensor.cpp
src/robotis_device/Dynamixel.cpp
src/robotis_device/robot.cpp
src/robotis_device/sensor.cpp
src/robotis_device/dynamixel.cpp
)
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
add_dependencies(robotis_device ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
# add_executable(robotis_device_node src/robotis_device_node.cpp)
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(robotis_device_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(robotis_device
${catkin_LIBRARIES}
)

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

@ -1,54 +1,84 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/*
* ControlTableItem.h
* control_table_item.h
*
* Created on: 2015. 12. 16.
* Author: zerom
*/
#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_CONTROLTABLEITEM_H_
#define ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_CONTROLTABLEITEM_H_
#ifndef ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_
#define ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_
#include <robotis_framework_common/RobotisDef.h>
#include <stdint.h>
namespace ROBOTIS
namespace robotis_framework
{
enum ACCESS_TYPE {
READ,
READ_WRITE
enum AccessType {
Read,
ReadWrite
};
enum MEMORY_TYPE {
EEPROM,
RAM
enum MemoryType {
EEPROM,
RAM
};
class ControlTableItem
{
public:
std::string item_name;
UINT16_T address;
ACCESS_TYPE access_type;
MEMORY_TYPE memory_type;
UINT8_T data_length;
INT32_T data_min_value;
INT32_T data_max_value;
bool is_signed;
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_;
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)
{ }
};
}
#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_CONTROLTABLEITEM_H_ */
#endif /* ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ */

57
robotis_device/include/robotis_device/device.h
View File

@ -1,37 +1,68 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/*
* Device.h
* device.h
*
* Created on: 2016. 5. 12.
* Author: zerom
*/
#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_
#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_
#ifndef ROBOTIS_DEVICE_DEVICE_H_
#define ROBOTIS_DEVICE_DEVICE_H_
#include <map>
#include <string>
#include <vector>
#include "ControlTableItem.h"
namespace ROBOTIS
#include "control_table_item.h"
namespace robotis_framework
{
class Device
{
public:
UINT8_T id;
float protocol_version;
std::string model_name;
std::string port_name;
uint8_t id_;
float protocol_version_;
std::string model_name_;
std::string port_name_;
std::map<std::string, ControlTableItem *> ctrl_table;
std::vector<ControlTableItem *> bulk_read_items;
std::map<std::string, ControlTableItem *> ctrl_table_;
std::vector<ControlTableItem *> bulk_read_items_;
virtual ~Device() { }
virtual ~Device() { }
};
}
#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_ */
#endif /* ROBOTIS_DEVICE_DEVICE_H_ */

95
robotis_device/include/robotis_device/dynamixel.h
View File

@ -1,61 +1,92 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/*
* Dynamixel.h
* dynamixel.h
*
* Created on: 2015. 12. 8.
* Author: zerom
*/
#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_
#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_
#ifndef ROBOTIS_DEVICE_DYNAMIXEL_H_
#define ROBOTIS_DEVICE_DYNAMIXEL_H_
#include <map>
#include <vector>
#include <string>
#include "Device.h"
#include "DynamixelState.h"
#include "ControlTableItem.h"
namespace ROBOTIS
#include "control_table_item.h"
#include "device.h"
#include "dynamixel_state.h"
namespace robotis_framework
{
class Dynamixel : public Device
{
public:
std::string ctrl_module_name;
DynamixelState *dxl_state;
std::string ctrl_module_name_;
DynamixelState *dxl_state_;
double current_ratio;
double velocity_ratio;
double current_ratio_;
double velocity_ratio_;
INT32_T value_of_0_radian_position;
INT32_T value_of_min_radian_position;
INT32_T value_of_max_radian_position;
double min_radian;
double max_radian;
int32_t value_of_0_radian_position_;
int32_t value_of_min_radian_position_;
int32_t value_of_max_radian_position_;
double min_radian_;
double max_radian_;
ControlTableItem *torque_enable_item;
ControlTableItem *present_position_item;
ControlTableItem *present_velocity_item;
ControlTableItem *present_current_item;
ControlTableItem *goal_position_item;
ControlTableItem *goal_velocity_item;
ControlTableItem *goal_current_item;
ControlTableItem *position_p_gain_item;
ControlTableItem *torque_enable_item_;
ControlTableItem *present_position_item_;
ControlTableItem *present_velocity_item_;
ControlTableItem *present_current_item_;
ControlTableItem *goal_position_item_;
ControlTableItem *goal_velocity_item_;
ControlTableItem *goal_current_item_;
ControlTableItem *position_p_gain_item_;
Dynamixel(int id, std::string model_name, float protocol_version);
Dynamixel(int id, std::string model_name, float protocol_version);
double ConvertValue2Radian(INT32_T value);
INT32_T ConvertRadian2Value(double radian);
double convertValue2Radian(int32_t value);
int32_t convertRadian2Value(double radian);
double ConvertValue2Velocity(INT32_T value);
INT32_T ConvertVelocity2Value(double velocity);
double convertValue2Velocity(int32_t value);
int32_t convertVelocity2Value(double velocity);
double ConvertValue2Current(INT16_T value);
INT16_T ConvertCurrent2Value(double torque);
double convertValue2Current(int16_t value);
int16_t convertCurrent2Value(double current);
};
}
#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_ */
#endif /* ROBOTIS_DEVICE_DYNAMIXEL_H_ */

89
robotis_device/include/robotis_device/dynamixel_state.h
View File

@ -1,57 +1,86 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/*
* DynamixelState.h
* dynamixel_state.h
*
* Created on: 2015. 12. 8.
* Author: zerom
*/
#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_
#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_
#ifndef ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_
#define ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_
#include <stdint.h>
#include "robotis_device/TimeStamp.h"
#include "robotis_framework_common/RobotisDef.h"
#include "time_stamp.h"
#define INDIRECT_DATA_1 "indirect_data_1"
#define INDIRECT_ADDRESS_1 "indirect_address_1"
namespace ROBOTIS
namespace robotis_framework
{
class DynamixelState
{
public:
TimeStamp update_time_stamp;
TimeStamp update_time_stamp_;
double present_position;
double present_velocity;
double present_current;
double goal_position;
double goal_velocity;
double goal_current;
double position_p_gain;
double present_position_;
double present_velocity_;
double present_current_;
double goal_position_;
double goal_velocity_;
double goal_current_;
double position_p_gain_;
std::map<std::string, UINT32_T> bulk_read_table;
std::map<std::string, uint32_t> bulk_read_table_;
double position_offset;
double position_offset_;
DynamixelState()
: update_time_stamp(0, 0),
present_position(0.0),
present_velocity(0.0),
present_current(0.0),
goal_position(0.0),
goal_velocity(0.0),
goal_current(0.0),
position_p_gain(0.0),
position_offset(0.0)
{
bulk_read_table.clear();
}
DynamixelState()
: update_time_stamp_(0, 0),
present_position_(0.0),
present_velocity_(0.0),
present_current_(0.0),
goal_position_(0.0),
goal_velocity_(0.0),
goal_current_(0.0),
position_p_gain_(0.0),
position_offset_(0.0)
{
bulk_read_table_.clear();
}
};
}
#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_ */
#endif /* ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ */

70
robotis_device/include/robotis_device/robot.h
View File

@ -1,38 +1,78 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/*
* Robot.h
* robot.h
*
* Created on: 2015. 12. 11.
* Author: zerom
*/
#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_ROBOT_H_
#define ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_ROBOT_H_
#ifndef ROBOTIS_DEVICE_ROBOT_H_
#define ROBOTIS_DEVICE_ROBOT_H_
#include <vector>
#include "Sensor.h"
#include "Dynamixel.h"
#include "dynamixel_sdk/PortHandler.h"
namespace ROBOTIS
#include "sensor.h"
#include "dynamixel.h"
#include "dynamixel_sdk/port_handler.h"
#define DYNAMIXEL "dynamixel"
#define SENSOR "sensor"
#define SESSION_PORT_INFO "port info"
#define SESSION_DEVICE_INFO "device info"
#define SESSION_TYPE_INFO "type info"
#define SESSION_CONTROL_TABLE "control table"
namespace robotis_framework
{
class Robot
{
public:
std::map<std::string, PortHandler *> ports; // string: port name
std::map<std::string, std::string> port_default_device; // port name, default device name
std::map<std::string, dynamixel::PortHandler *> ports_; // string: port name
std::map<std::string, std::string> port_default_device_; // port name, default device name
std::map<std::string, Dynamixel *> dxls; // string: joint name
std::map<std::string, Sensor *> sensors; // string: sensor name
std::map<std::string, Dynamixel *> dxls_; // string: joint name
std::map<std::string, Sensor *> sensors_; // string: sensor name
Robot(std::string robot_file_path, std::string dev_desc_dir_path);
Robot(std::string robot_file_path, std::string dev_desc_dir_path);
Sensor *getSensor(std::string path, int id, std::string port, float protocol_version);
Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version);
Sensor *getSensor(std::string path, int id, std::string port, float protocol_version);
Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version);
};
}
#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_ROBOT_H_ */
#endif /* ROBOTIS_DEVICE_ROBOT_H_ */

51
robotis_device/include/robotis_device/sensor.h
View File

@ -1,32 +1,63 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/*
* Sensor.h
* sensor.h
*
* Created on: 2015. 12. 16.
* Author: zerom
*/
#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_
#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_
#ifndef ROBOTIS_DEVICE_SENSOR_H_
#define ROBOTIS_DEVICE_SENSOR_H_
#include <map>
#include <string>
#include <stdint.h>
#include "Device.h"
#include "SensorState.h"
#include "ControlTableItem.h"
namespace ROBOTIS
#include "device.h"
#include "sensor_state.h"
#include "control_table_item.h"
namespace robotis_framework
{
class Sensor : public Device
{
public:
SensorState *sensor_state;
SensorState *sensor_state_;
Sensor(int id, std::string model_name, float protocol_version);
Sensor(int id, std::string model_name, float protocol_version);
};
}
#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_ */
#endif /* ROBOTIS_DEVICE_SENSOR_H_ */

57
robotis_device/include/robotis_device/sensor_state.h
View File

@ -1,35 +1,64 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/*
* SensorState.h
* sensor_state.h
*
* Created on: 2016. 5. 16.
* Author: zerom
*/
#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_
#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_
#ifndef ROBOTIS_DEVICE_SENSOR_STATE_H_
#define ROBOTIS_DEVICE_SENSOR_STATE_H_
#include "robotis_device/TimeStamp.h"
#include "robotis_framework_common/RobotisDef.h"
#include "time_stamp.h"
namespace ROBOTIS
namespace robotis_framework
{
class SensorState
{
public:
TimeStamp update_time_stamp;
TimeStamp update_time_stamp_;
std::map<std::string, UINT32_T> bulk_read_table;
std::map<std::string, uint32_t> bulk_read_table_;
SensorState()
: update_time_stamp(0, 0)
{
bulk_read_table.clear();
}
SensorState()
: update_time_stamp_(0, 0)
{
bulk_read_table_.clear();
}
};
}
#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_ */
#endif /* ROBOTIS_DEVICE_SENSOR_STATE_H_ */

50
robotis_device/include/robotis_device/time_stamp.h
View File

@ -1,25 +1,59 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/*
* TimeStamp.h
* time_stamp.h
*
* Created on: 2016. 5. 16.
* Author: zerom
*/
#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_
#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_
#ifndef ROBOTIS_DEVICE_TIME_STAMP_H_
#define ROBOTIS_DEVICE_TIME_STAMP_H_
namespace ROBOTIS
namespace robotis_framework
{
class TimeStamp {
public:
long sec;
long nsec;
TimeStamp(long sec, long nsec) : sec(sec), nsec(nsec) { }
long sec_;
long nsec_;
TimeStamp(long sec, long nsec)
: sec_(sec),
nsec_(nsec)
{ }
};
}
#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_ */
#endif /* ROBOTIS_DEVICE_TIME_STAMP_H_ */

9
robotis_device/package.xml
View File

@ -6,16 +6,12 @@
<maintainer email="zerom@robotis.com">robotis</maintainer>
<license>BSD</license>
<!-- <url type="website">http://wiki.ros.org/robotis_device</url> -->
<author email="zerom@robotis.com">robotis</author>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
@ -26,9 +22,4 @@
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>dynamixel_sdk</run_depend>
<export>
</export>
</package>

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

@ -1,111 +1,147 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/*
* Dynamixel.cpp
* dynamixel.cpp
*
* Created on: 2015. 12. 8.
* Author: zerom
*/
#include "robotis_device/Dynamixel.h"
#include "robotis_device/dynamixel.h"
using namespace ROBOTIS;
using namespace robotis_framework;
Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version)
: ctrl_module_name("none"),
current_ratio(1.0),
velocity_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)
: ctrl_module_name_("none"),
current_ratio_(1.0),
velocity_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)
{
this->id = id;
this->model_name = model_name;
this->port_name = "";
this->protocol_version = protocol_version;
this->id_ = id;
this->model_name_ = model_name;
this->port_name_ = "";
this->protocol_version_ = protocol_version;
ctrl_table.clear();
dxl_state = new DynamixelState();
ctrl_table_.clear();
dxl_state_ = new DynamixelState();
bulk_read_items.clear();
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(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)
{
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);
}
double radian = 0.0;
if (value > value_of_0_radian_position_)
{
if (max_radian_ <= 0)
return max_radian_;
if(_radian > max_radian)
return max_radian;
else if(_radian < min_radian)
return min_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_)
{
if (min_radian_ >= 0)
return min_radian_;
return _radian;
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_;
return radian;
}
INT32_T Dynamixel::ConvertRadian2Value(double radian)
int32_t Dynamixel::convertRadian2Value(double radian)
{
//return radian * value_of_max_radian_position / max_radian;
int32_t value = 0;
if (radian > 0)
{
if (value_of_max_radian_position_ <= value_of_0_radian_position_)
return value_of_max_radian_position_;
INT32_T _value = 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)
{
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 = value_of_0_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)
{
if (value_of_min_radian_position_ >= value_of_0_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;
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_;
return _value;
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)
double Dynamixel::convertValue2Velocity(int32_t value)
{
return (double)value * velocity_ratio;
return (double) value * velocity_ratio_;
}
INT32_T Dynamixel::ConvertVelocity2Value(double velocity)
int32_t Dynamixel::convertVelocity2Value(double velocity)
{
return (INT32_T)(velocity / velocity_ratio);;
return (int32_t) (velocity / velocity_ratio_);;
}
double Dynamixel::ConvertValue2Current(INT16_T value)
double Dynamixel::convertValue2Current(int16_t value)
{
return (double)value * current_ratio;
return (double) value * current_ratio_;
}
INT16_T Dynamixel::ConvertCurrent2Value(double torque)
int16_t Dynamixel::convertCurrent2Value(double current)
{
return (INT16_T)(torque / current_ratio);
return (int16_t) (current / current_ratio_);
}

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

@ -1,5 +1,35 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/*
* Robot.cpp
* robot.cpp
*
* Created on: 2015. 12. 11.
* Author: zerom
@ -8,404 +38,420 @@
#include <fstream>
#include <iostream>
#include <algorithm>
#include "../include/robotis_device/Robot.h"
using namespace ROBOTIS;
#include "robotis_device/robot.h"
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;
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))));
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());
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());
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) {
std::vector<std::string> tokens;
std::size_t start = 0, end = 0;
while((end = text.find(sep, start)) != (std::string::npos)) {
tokens.push_back(text.substr(start, end - start));
trim(tokens.back());
start = end + 1;
}
tokens.push_back(text.substr(start));
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))
{
tokens.push_back(text.substr(start, end - start));
trim(tokens.back());
return tokens;
start = end + 1;
}
tokens.push_back(text.substr(start));
trim(tokens.back());
return tokens;
}
Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
{
if(dev_desc_dir_path.compare(dev_desc_dir_path.size()-1, 1, "/") != 0)
dev_desc_dir_path += "/";
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())
std::ifstream file(robot_file_path.c_str());
if (file.is_open())
{
std::string session = "";
std::string input_str;
while (!file.eof())
{
std::string session = "";
std::string input_str;
while(!file.eof())
std::getline(file, input_str);
// remove comment ( # )
std::size_t pos = input_str.find("#");
if (pos != std::string::npos)
input_str = input_str.substr(0, pos);
// trim
input_str = trim(input_str);
// find session
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);
session = trim(input_str);
continue;
}
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;
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)
{
std::vector<std::string> tokens = split(input_str, '|');
if (tokens.size() != 7)
continue;
if (tokens[0] == DYNAMIXEL)
{
std::getline(file, input_str);
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];
// remove comment ( # )
std::size_t pos = input_str.find("#");
if(pos != std::string::npos)
input_str = input_str.substr(0, pos);
dxls_[dev_name] = getDynamixel(file_path, id, port, protocol);
// trim
input_str = trim(input_str);
// find session
if(!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size()-1, 1, "]"))
Dynamixel *dxl = dxls_[dev_name];
std::vector<std::string> sub_tokens = split(tokens[6], ',');
if (sub_tokens.size() > 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
{
input_str = input_str.substr(1, input_str.size()-2);
std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower);
session = trim(input_str);
continue;
}
uint16_t indirect_data_addr = dxl->ctrl_table_[INDIRECT_DATA_1]->address_;
for (int _i = 0; _i < sub_tokens.size(); _i++)
{
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]];
if(session == "port info")
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
{
std::vector<std::string> tokens = split(input_str, '|');
if(tokens.size() != 3)
continue;
std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" << std::endl;
ports[tokens[0]] = (PortHandler*)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 == "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];
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)
{
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++)
{
_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]];
_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
{
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]]);
}
}
}
}
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)
{
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++)
{
_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]];
_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
{
for(int _i = 0; _i < sub_tokens.size(); _i++)
_sensor->bulk_read_items.push_back(_sensor->ctrl_table[sub_tokens[_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]]);
}
}
}
}
file.close();
}
else
{
std::cout << "Unable to open file : " + robot_file_path << std::endl;
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)
{
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++)
{
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]];
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
{
for (int i = 0; i < sub_tokens.size(); i++)
sensor->bulk_read_items_.push_back(sensor->ctrl_table_[sub_tokens[i]]);
}
}
}
}
}
file.close();
}
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 *_sensor;
Sensor *sensor;
// load file model_name.device
std::ifstream file(path.c_str());
if(file.is_open())
// load file model_name.device
std::ifstream file(path.c_str());
if (file.is_open())
{
std::string session = "";
std::string input_str;
while (!file.eof())
{
std::string _session = "";
std::string _input_str;
std::getline(file, input_str);
while(!file.eof())
{
std::getline(file, _input_str);
// remove comment ( # )
std::size_t pos = input_str.find("#");
if (pos != std::string::npos)
input_str = input_str.substr(0, pos);
// remove comment ( # )
std::size_t pos = _input_str.find("#");
if(pos != std::string::npos)
_input_str = _input_str.substr(0, pos);
// trim
input_str = trim(input_str);
if (input_str == "")
continue;
// trim
_input_str = trim(_input_str);
if(_input_str == "")
continue;
// find _session
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);
session = trim(input_str);
continue;
}
// find _session
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);
_session = trim(_input_str);
continue;
}
if (session == SESSION_DEVICE_INFO)
{
std::vector<std::string> tokens = split(input_str, '=');
if (tokens.size() != 2)
continue;
if(_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)
{
std::vector<std::string> tokens = split(input_str, '|');
if (tokens.size() != 8)
continue;
if(tokens[0] == "model_name")
_sensor = new Sensor(id, tokens[1], protocol_version);
}
else if(_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());
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());
if(tokens[3] == "R")
item->access_type = READ;
else if(tokens[3] == "RW")
item->access_type = READ_WRITE;
if(tokens[4] == "EEPROM")
item->memory_type = EEPROM;
else if(tokens[4] == "RAM")
item->memory_type = RAM;
item->data_min_value = std::atol(tokens[5].c_str());
item->data_max_value = std::atol(tokens[6].c_str());
if(tokens[7] == "Y")
item->is_signed = true;
else if(tokens[7] == "N")
item->is_signed = false;
_sensor->ctrl_table[tokens[1]] = item;
}
}
_sensor->port_name = port;
if (tokens[3] == "R")
item->access_type_ = Read;
else if (tokens[3] == "RW")
item->access_type_ = ReadWrite;
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();
if (tokens[4] == "EEPROM")
item->memory_type_ = EEPROM;
else if (tokens[4] == "RAM")
item->memory_type_ = RAM;
item->data_min_value_ = std::atol(tokens[5].c_str());
item->data_max_value_ = std::atol(tokens[6].c_str());
if (tokens[7] == "Y")
item->is_signed_ = true;
else if (tokens[7] == "N")
item->is_signed_ = false;
sensor->ctrl_table_[tokens[1]] = item;
}
}
else
std::cout << "Unable to open file : " + path << std::endl;
sensor->port_name_ = port;
return _sensor;
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
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 *_dxl;
Dynamixel *dxl;
// load file model_name.device
std::ifstream file(path.c_str());
if(file.is_open())
// load file model_name.device
std::ifstream file(path.c_str());
if (file.is_open())
{
std::string session = "";
std::string input_str;
std::string torque_enable_item_name = "";
std::string present_position_item_name = "";
std::string present_velocity_item_name = "";
std::string present_current_item_name = "";
std::string goal_position_item_name = "";
std::string goal_velocity_item_name = "";
std::string goal_current_item_name = "";
while (!file.eof())
{
std::string _session = "";
std::string _input_str;
std::getline(file, input_str);
std::string _torque_enable_item_name = "";
std::string _present_position_item_name = "";
std::string _present_velocity_item_name = "";
std::string _present_current_item_name = "";
std::string _goal_position_item_name = "";
std::string _goal_velocity_item_name = "";
std::string _goal_current_item_name = "";
// remove comment ( # )
std::size_t pos = input_str.find("#");
if (pos != std::string::npos)
input_str = input_str.substr(0, pos);
while(!file.eof())
{
std::getline(file, _input_str);
// trim
input_str = trim(input_str);
if (input_str == "")
continue;
// remove comment ( # )
std::size_t pos = _input_str.find("#");
if(pos != std::string::npos)
_input_str = _input_str.substr(0, pos);
// find session
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);
session = trim(input_str);
continue;
}
// trim
_input_str = trim(_input_str);
if(_input_str == "")
continue;
if (session == SESSION_DEVICE_INFO)
{
std::vector<std::string> tokens = split(input_str, '=');
if (tokens.size() != 2)
continue;
// find _session
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);
_session = trim(_input_str);
continue;
}
if (tokens[0] == "model_name")
dxl = new Dynamixel(id, tokens[1], protocol_version);
}
else if (session == SESSION_TYPE_INFO)
{
std::vector<std::string> tokens = split(input_str, '=');
if (tokens.size() != 2)
continue;
if(_session == "device info")
{
std::vector<std::string> tokens = split(_input_str, '=');
if(tokens.size() != 2)
continue;
if (tokens[0] == "current_ratio")
dxl->current_ratio_ = std::atof(tokens[1].c_str());
else if (tokens[0] == "velocity_ratio")
dxl->velocity_ratio_ = std::atof(tokens[1].c_str());
if(tokens[0] == "model_name")
_dxl = new Dynamixel(id, tokens[1], protocol_version);
}
else if(_session == "type info")
{
std::vector<std::string> tokens = split(_input_str, '=');
if(tokens.size() != 2)
continue;
else if (tokens[0] == "value_of_0_radian_position")
dxl->value_of_0_radian_position_ = std::atoi(tokens[1].c_str());
else if (tokens[0] == "value_of_min_radian_position")
dxl->value_of_min_radian_position_ = std::atoi(tokens[1].c_str());
else if (tokens[0] == "value_of_max_radian_position")
dxl->value_of_max_radian_position_ = std::atoi(tokens[1].c_str());
else if (tokens[0] == "min_radian")
dxl->min_radian_ = std::atof(tokens[1].c_str());
else if (tokens[0] == "max_radian")
dxl->max_radian_ = std::atof(tokens[1].c_str());
if(tokens[0] == "current_ratio")
_dxl->current_ratio = std::atof(tokens[1].c_str());
else if(tokens[0] == "velocity_ratio")
_dxl->velocity_ratio = std::atof(tokens[1].c_str());
else if (tokens[0] == "torque_enable_item_name")
torque_enable_item_name = tokens[1];
else if (tokens[0] == "present_position_item_name")
present_position_item_name = tokens[1];
else if (tokens[0] == "present_velocity_item_name")
present_velocity_item_name = tokens[1];
else if (tokens[0] == "present_current_item_name")
present_current_item_name = tokens[1];
else if (tokens[0] == "goal_position_item_name")
goal_position_item_name = tokens[1];
else if (tokens[0] == "goal_velocity_item_name")
goal_velocity_item_name = tokens[1];
else if (tokens[0] == "goal_current_item_name")
goal_current_item_name = tokens[1];
}
else if (session == SESSION_CONTROL_TABLE)
{
std::vector<std::string> tokens = split(input_str, '|');
if (tokens.size() != 8)
continue;
else if(tokens[0] == "value_of_0_radian_position")
_dxl->value_of_0_radian_position = std::atoi(tokens[1].c_str());
else if(tokens[0] == "value_of_min_radian_position")
_dxl->value_of_min_radian_position = std::atoi(tokens[1].c_str());
else if(tokens[0] == "value_of_max_radian_position")
_dxl->value_of_max_radian_position = std::atoi(tokens[1].c_str());
else if(tokens[0] == "min_radian")
_dxl->min_radian = std::atof(tokens[1].c_str());
else if(tokens[0] == "max_radian")
_dxl->max_radian = std::atof(tokens[1].c_str());
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());
else if(tokens[0] == "torque_enable_item_name")
_torque_enable_item_name = tokens[1];
else if(tokens[0] == "present_position_item_name")
_present_position_item_name = tokens[1];
else if(tokens[0] == "present_velocity_item_name")
_present_velocity_item_name = tokens[1];
else if(tokens[0] == "present_current_item_name")
_present_current_item_name = tokens[1];
else if(tokens[0] == "goal_position_item_name")
_goal_position_item_name = tokens[1];
else if(tokens[0] == "goal_velocity_item_name")
_goal_velocity_item_name = tokens[1];
else if(tokens[0] == "goal_current_item_name")
_goal_current_item_name = tokens[1];
}
else if(_session == "control table")
{
std::vector<std::string> tokens = split(_input_str, '|');
if(tokens.size() != 8)
continue;
if (tokens[3] == "R")
item->access_type_ = Read;
else if (tokens[3] == "RW")
item->access_type_ = ReadWrite;
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());
if(tokens[3] == "R")
item->access_type = READ;
else if(tokens[3] == "RW")
item->access_type = READ_WRITE;
if(tokens[4] == "EEPROM")
item->memory_type = EEPROM;
else if(tokens[4] == "RAM")
item->memory_type = RAM;
item->data_min_value = std::atol(tokens[5].c_str());
item->data_max_value = std::atol(tokens[6].c_str());
if(tokens[7] == "Y")
item->is_signed = true;
else if(tokens[7] == "N")
item->is_signed = false;
_dxl->ctrl_table[tokens[1]] = item;
}
}
_dxl->port_name = port;
if (tokens[4] == "EEPROM")
item->memory_type_ = EEPROM;
else if (tokens[4] == "RAM")
item->memory_type_ = RAM;
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];
if(_dxl->ctrl_table[_present_velocity_item_name] != NULL)
_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)
_dxl->goal_position_item = _dxl->ctrl_table[_goal_position_item_name];
if(_dxl->ctrl_table[_goal_velocity_item_name] != NULL)
_dxl->goal_velocity_item = _dxl->ctrl_table[_goal_velocity_item_name];
if(_dxl->ctrl_table[_goal_current_item_name] != NULL)
_dxl->goal_current_item = _dxl->ctrl_table[_goal_current_item_name];
item->data_min_value_ = std::atol(tokens[5].c_str());
item->data_max_value_ = std::atol(tokens[6].c_str());
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();
if (tokens[7] == "Y")
item->is_signed_ = true;
else if (tokens[7] == "N")
item->is_signed_ = false;
dxl->ctrl_table_[tokens[1]] = item;
}
}
else
std::cout << "Unable to open file : " + path << std::endl;
dxl->port_name_ = port;
return _dxl;
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];
if (dxl->ctrl_table_[present_velocity_item_name] != NULL)
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)
dxl->goal_position_item_ = dxl->ctrl_table_[goal_position_item_name];
if (dxl->ctrl_table_[goal_velocity_item_name] != NULL)
dxl->goal_velocity_item_ = dxl->ctrl_table_[goal_velocity_item_name];
if (dxl->ctrl_table_[goal_current_item_name] != NULL)
dxl->goal_current_item_ = dxl->ctrl_table_[goal_current_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;
file.close();
}
else
std::cout << "Unable to open file : " + path << std::endl;
return dxl;
}

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

@ -1,23 +1,53 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/*
* Sensor.cpp
* sensor.cpp
*
* Created on: 2016. 5. 11.
* Author: zerom
*/
#include "robotis_device/Sensor.h"
#include "robotis_device/sensor.h"
using namespace ROBOTIS;
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();
this->id_ = id;
this->model_name_ = model_name;
this->port_name_ = "";
this->protocol_version_ = protocol_version;
ctrl_table_.clear();
sensor_state = new SensorState();
sensor_state_ = new SensorState();
bulk_read_items.clear();
bulk_read_items_.clear();
}