diff --git a/robotis_controller/include/robotis_controller/RobotisController.h b/robotis_controller/include/robotis_controller/RobotisController.h index c28256c..2fa4b48 100644 --- a/robotis_controller/include/robotis_controller/RobotisController.h +++ b/robotis_controller/include/robotis_controller/RobotisController.h @@ -37,11 +37,9 @@ enum CONTROLLER_MODE DIRECT_CONTROL_MODE }; -class RobotisController +class RobotisController : public Singleton { private: - static RobotisController *unique_instance_; - boost::thread queue_thread_; boost::thread gazebo_thread_; boost::thread set_module_thread_; @@ -59,8 +57,6 @@ private: std::map sensor_result_; - RobotisController(); - void QueueThread(); void GazeboThread(); void SetCtrlModuleThread(std::string ctrl_module); @@ -91,7 +87,8 @@ public: std::map gazebo_joint_pub; static void *ThreadProc(void *param); - static RobotisController *GetInstance() { return unique_instance_; } + + RobotisController(); bool Initialize(const std::string robot_file_path, const std::string init_file_path); void DeviceInit(const std::string init_file_path); diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/RobotisController.cpp index 60cdf91..a2d5251 100644 --- a/robotis_controller/src/robotis_controller/RobotisController.cpp +++ b/robotis_controller/src/robotis_controller/RobotisController.cpp @@ -12,8 +12,6 @@ using namespace ROBOTIS; -RobotisController *RobotisController::unique_instance_ = new RobotisController(); - RobotisController::RobotisController() : is_timer_running_(false), stop_timer_(false), @@ -586,70 +584,105 @@ void RobotisController::Process() // if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; // -> save to Robot->dxls[]->dynamixel_state.present_position - for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) + if(robot->dxls.size() > 0) { - UINT32_T _data = 0; - - std::string _port_name = dxl_it->second->port_name; - std::string _joint_name = dxl_it->first; - Dynamixel *_dxl = dxl_it->second; - - _present_state.header.stamp = ros::Time::now(); - _goal_state.header.stamp = _present_state.header.stamp; - - if(gazebo_mode == false) + for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) { - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) - { - ControlTableItem *_item = _dxl->bulk_read_items[_i]; - if(port_to_bulk_read[_port_name]->IsAvailable(_dxl->id, _item->address, _item->data_length) == true) - { - _data = port_to_bulk_read[_port_name]->GetData(_dxl->id, _item->address, _item->data_length); + UINT32_T _data = 0; - // TODO: change dxl_state - if(_dxl->present_position_item != 0 && _item->item_name == _dxl->present_position_item->item_name) - _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset - else if(_dxl->present_velocity_item != 0 && _item->item_name == _dxl->present_velocity_item->item_name) - _dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_data); - else if(_dxl->present_current_item != 0 && _item->item_name == _dxl->present_current_item->item_name) - _dxl->dxl_state->present_current = _dxl->ConvertValue2Current(_data); - else if(_dxl->goal_position_item != 0 && _item->item_name == _dxl->goal_position_item->item_name) - _dxl->dxl_state->goal_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset - else if(_dxl->goal_velocity_item != 0 && _item->item_name == _dxl->goal_velocity_item->item_name) - _dxl->dxl_state->goal_velocity = _dxl->ConvertValue2Velocity(_data); - else if(_dxl->goal_current_item != 0 && _item->item_name == _dxl->goal_current_item->item_name) - _dxl->dxl_state->goal_current = _dxl->ConvertValue2Current(_data); - else - _dxl->dxl_state->bulk_read_table[_item->item_name] = _data; + std::string _port_name = dxl_it->second->port_name; + std::string _joint_name = dxl_it->first; + Dynamixel *_dxl = dxl_it->second; + + _present_state.header.stamp = ros::Time::now(); + _goal_state.header.stamp = _present_state.header.stamp; + + if(gazebo_mode == false && _dxl->bulk_read_items.size() > 0) + { + for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) + { + ControlTableItem *_item = _dxl->bulk_read_items[_i]; + if(port_to_bulk_read[_port_name]->IsAvailable(_dxl->id, _item->address, _item->data_length) == true) + { + _data = port_to_bulk_read[_port_name]->GetData(_dxl->id, _item->address, _item->data_length); + + // TODO: change dxl_state + if(_dxl->present_position_item != 0 && _item->item_name == _dxl->present_position_item->item_name) + _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset + else if(_dxl->present_velocity_item != 0 && _item->item_name == _dxl->present_velocity_item->item_name) + _dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_data); + else if(_dxl->present_current_item != 0 && _item->item_name == _dxl->present_current_item->item_name) + _dxl->dxl_state->present_current = _dxl->ConvertValue2Current(_data); + else if(_dxl->goal_position_item != 0 && _item->item_name == _dxl->goal_position_item->item_name) + _dxl->dxl_state->goal_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset + else if(_dxl->goal_velocity_item != 0 && _item->item_name == _dxl->goal_velocity_item->item_name) + _dxl->dxl_state->goal_velocity = _dxl->ConvertValue2Velocity(_data); + else if(_dxl->goal_current_item != 0 && _item->item_name == _dxl->goal_current_item->item_name) + _dxl->dxl_state->goal_current = _dxl->ConvertValue2Current(_data); + else + _dxl->dxl_state->bulk_read_table[_item->item_name] = _data; + } } + + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + _dxl->dxl_state->update_time_stamp = TimeStamp(_present_state.header.stamp.sec, _present_state.header.stamp.nsec); } - // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - _dxl->dxl_state->update_time_stamp = TimeStamp(_present_state.header.stamp.sec, _present_state.header.stamp.nsec); + _present_state.name.push_back(_joint_name); + // TODO: should be converted to rad, rad/s, Nm + _present_state.position.push_back(_dxl->dxl_state->present_position); + _present_state.velocity.push_back(_dxl->dxl_state->present_velocity); + _present_state.effort.push_back(_dxl->dxl_state->present_current); + + _goal_state.name.push_back(_joint_name); + _goal_state.position.push_back(_dxl->dxl_state->goal_position); + _goal_state.velocity.push_back(_dxl->dxl_state->goal_velocity); + _goal_state.effort.push_back(_dxl->dxl_state->goal_current); } - - _present_state.name.push_back(_joint_name); - // TODO: should be converted to rad, rad/s, Nm - _present_state.position.push_back(_dxl->dxl_state->present_position); - _present_state.velocity.push_back(_dxl->dxl_state->present_velocity); - _present_state.effort.push_back(_dxl->dxl_state->present_current); - - _goal_state.name.push_back(_joint_name); - _goal_state.position.push_back(_dxl->dxl_state->goal_position); - _goal_state.velocity.push_back(_dxl->dxl_state->goal_velocity); - _goal_state.effort.push_back(_dxl->dxl_state->goal_current); } + // -> publish present joint_states & goal joint states topic present_joint_state_pub.publish(_present_state); goal_joint_state_pub.publish(_goal_state); + if(robot->sensors.size() > 0) + { + for(std::map::iterator sensor_it = robot->sensors.begin(); sensor_it != robot->sensors.end(); sensor_it++) + { + UINT32_T _data = 0; + + std::string _port_name = sensor_it->second->port_name; + std::string _sensor_name = sensor_it->first; + Sensor *_sensor = sensor_it->second; + + if(gazebo_mode == false && _sensor->bulk_read_items.size() > 0) + { + for(int _i = 0; _i < _sensor->bulk_read_items.size(); _i++) + { + ControlTableItem *_item = _sensor->bulk_read_items[_i]; + if(port_to_bulk_read[_port_name]->IsAvailable(_sensor->id, _item->address, _item->data_length) == true) + { + _data = port_to_bulk_read[_port_name]->GetData(_sensor->id, _item->address, _item->data_length); + + // TODO: change sensor_state + _sensor->sensor_state->bulk_read_table[_item->item_name] = _data; + } + } + + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + ros::Time _now = ros::Time::now(); + _sensor->sensor_state->update_time_stamp = TimeStamp(_now.sec, _now.nsec); + } + } + } + // Call SensorModule Process() // -> for loop : call SensorModule list -> Process() if(sensor_modules_.size() > 0) { for(std::list::iterator _module_it = sensor_modules_.begin(); _module_it != sensor_modules_.end(); _module_it++) { - (*_module_it)->Process(robot->dxls); + (*_module_it)->Process(robot->dxls, robot->sensors); for(std::map::iterator _it = (*_module_it)->result.begin(); _it != (*_module_it)->result.end(); _it++) sensor_result_[_it->first] = _it->second; diff --git a/robotis_device/include/robotis_device/DynamixelState.h b/robotis_device/include/robotis_device/DynamixelState.h index f5df513..29212c4 100644 --- a/robotis_device/include/robotis_device/DynamixelState.h +++ b/robotis_device/include/robotis_device/DynamixelState.h @@ -5,11 +5,13 @@ * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXELSTATE_H_ -#define ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXELSTATE_H_ +#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_ +#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_ #include -#include + +#include "robotis_device/TimeStamp.h" +#include "robotis_framework_common/RobotisDef.h" #define INDIRECT_DATA_1 "indirect_data_1" #define INDIRECT_ADDRESS_1 "indirect_address_1" @@ -17,13 +19,6 @@ namespace ROBOTIS { -class TimeStamp { -public: - long sec; - long nsec; - TimeStamp(long sec, long nsec) : sec(sec), nsec(nsec) { } -}; - class DynamixelState { public: @@ -57,4 +52,4 @@ public: } -#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXELSTATE_H_ */ +#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_ */ diff --git a/robotis_device/include/robotis_device/Sensor.h b/robotis_device/include/robotis_device/Sensor.h index 78de619..54df44f 100644 --- a/robotis_device/include/robotis_device/Sensor.h +++ b/robotis_device/include/robotis_device/Sensor.h @@ -12,6 +12,7 @@ #include #include #include "Device.h" +#include "SensorState.h" #include "ControlTableItem.h" namespace ROBOTIS @@ -19,8 +20,9 @@ namespace ROBOTIS class Sensor : public Device { - std::map sensed_value; public: + SensorState *sensor_state; + Sensor(int id, std::string model_name, float protocol_version); }; diff --git a/robotis_device/include/robotis_device/SensorState.h b/robotis_device/include/robotis_device/SensorState.h new file mode 100644 index 0000000..4137b14 --- /dev/null +++ b/robotis_device/include/robotis_device/SensorState.h @@ -0,0 +1,35 @@ +/* + * SensorState.h + * + * Created on: 2016. 5. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_ +#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_ + + +#include "robotis_device/TimeStamp.h" +#include "robotis_framework_common/RobotisDef.h" + +namespace ROBOTIS +{ + +class SensorState +{ +public: + TimeStamp update_time_stamp; + + std::map bulk_read_table; + + SensorState() + : update_time_stamp(0, 0) + { + bulk_read_table.clear(); + } +}; + +} + + +#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_ */ diff --git a/robotis_device/include/robotis_device/TimeStamp.h b/robotis_device/include/robotis_device/TimeStamp.h new file mode 100644 index 0000000..17b749a --- /dev/null +++ b/robotis_device/include/robotis_device/TimeStamp.h @@ -0,0 +1,25 @@ +/* + * TimeStamp.h + * + * Created on: 2016. 5. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_ +#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_ + + +namespace ROBOTIS +{ + +class TimeStamp { +public: + long sec; + long nsec; + TimeStamp(long sec, long nsec) : sec(sec), nsec(nsec) { } +}; + +} + + +#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_ */ diff --git a/robotis_device/src/robotis_device/Sensor.cpp b/robotis_device/src/robotis_device/Sensor.cpp index afe4f9f..db95494 100644 --- a/robotis_device/src/robotis_device/Sensor.cpp +++ b/robotis_device/src/robotis_device/Sensor.cpp @@ -17,5 +17,7 @@ Sensor::Sensor(int id, std::string model_name, float protocol_version) this->protocol_version = protocol_version; ctrl_table.clear(); + sensor_state = new SensorState(); + bulk_read_items.clear(); } diff --git a/robotis_framework_common/include/robotis_framework_common/MotionModule.h b/robotis_framework_common/include/robotis_framework_common/MotionModule.h index 18d962e..a305a6a 100644 --- a/robotis_framework_common/include/robotis_framework_common/MotionModule.h +++ b/robotis_framework_common/include/robotis_framework_common/MotionModule.h @@ -14,6 +14,7 @@ #include "robotis_device/Robot.h" #include "robotis_device/Dynamixel.h" +#include "robotis_framework_common/Singleton.h" namespace ROBOTIS { diff --git a/robotis_framework_common/include/robotis_framework_common/SensorModule.h b/robotis_framework_common/include/robotis_framework_common/SensorModule.h index d34bfbb..6111391 100644 --- a/robotis_framework_common/include/robotis_framework_common/SensorModule.h +++ b/robotis_framework_common/include/robotis_framework_common/SensorModule.h @@ -14,6 +14,7 @@ #include "robotis_device/Robot.h" #include "robotis_device/Dynamixel.h" +#include "robotis_framework_common/Singleton.h" namespace ROBOTIS { @@ -28,7 +29,7 @@ public: virtual ~SensorModule() { } virtual void Initialize(const int control_cycle_msec, Robot *robot) = 0; - virtual void Process(std::map dxls) = 0; + virtual void Process(std::map dxls, std::map sensors) = 0; }; } diff --git a/robotis_framework_common/include/robotis_framework_common/Singleton.h b/robotis_framework_common/include/robotis_framework_common/Singleton.h new file mode 100644 index 0000000..2b4c7d8 --- /dev/null +++ b/robotis_framework_common/include/robotis_framework_common/Singleton.h @@ -0,0 +1,49 @@ +/* + * Singleton.h + * + * Created on: 2016. 5. 17. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ +#define ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ + + +namespace ROBOTIS +{ + +template +class Singleton +{ +private: + static T *unique_instance_; + +protected: + Singleton() { } + Singleton(Singleton const&) { } + Singleton& operator=(Singleton const&) { return *this; } + +public: + static T* GetInstance() + { + if(unique_instance_ == NULL) + unique_instance_ = new T; + return unique_instance_; + } + + static void DestroyInstance() + { + if(unique_instance_) + { + delete unique_instance_; + unique_instance_ = NULL; + } + } +}; + +template T* Singleton::unique_instance_ = NULL; + +} + + +#endif /* ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ */