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

- add SensorState

Browse Source 
- add Singleton template
This commit is contained in:
ROBOTIS-zerom
2016-05-17 20:41:14 +09:00
parent 9e1300eeda
commit 2f18b587de
10 changed files with 207 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
robotis_controller/include/robotis_controller/RobotisController.h
View File

@@ -37,11 +37,9 @@ enum CONTROLLER_MODE
DIRECT_CONTROL_MODE
};
class RobotisController
class RobotisController : public Singleton<RobotisController>
{
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<std::string, double> sensor_result_;
RobotisController();
void QueueThread();
void GazeboThread();
void SetCtrlModuleThread(std::string ctrl_module);
@@ -91,7 +87,8 @@ public:
std::map<std::string, ros::Publisher> 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);

129
robotis_controller/src/robotis_controller/RobotisController.cpp
View File

@@ -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<std::string, Dynamixel *>::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<std::string, Dynamixel *>::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<std::string, Sensor *>::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<SensorModule*>::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<std::string, double>::iterator _it = (*_module_it)->result.begin(); _it != (*_module_it)->result.end(); _it++)
sensor_result_[_it->first] = _it->second;

17
robotis_device/include/robotis_device/DynamixelState.h
View File

@@ -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 <stdint.h>
#include <robotis_framework_common/RobotisDef.h>
#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_ */

4
robotis_device/include/robotis_device/Sensor.h
View File

@@ -12,6 +12,7 @@
#include <string>
#include <stdint.h>
#include "Device.h"
#include "SensorState.h"
#include "ControlTableItem.h"
namespace ROBOTIS
@@ -19,8 +20,9 @@ namespace ROBOTIS
class Sensor : public Device
{
std::map<std::string, double> sensed_value;
public:
SensorState *sensor_state;
Sensor(int id, std::string model_name, float protocol_version);
};

35
robotis_device/include/robotis_device/SensorState.h Normal file
View File

@@ -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<std::string, UINT32_T> bulk_read_table;
SensorState()
: update_time_stamp(0, 0)
{
bulk_read_table.clear();
}
};
}
#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_ */

25
robotis_device/include/robotis_device/TimeStamp.h Normal file
View File

@@ -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_ */

2
robotis_device/src/robotis_device/Sensor.cpp
View File

@@ -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();
}

1
robotis_framework_common/include/robotis_framework_common/MotionModule.h
View File

@@ -14,6 +14,7 @@
#include "robotis_device/Robot.h"
#include "robotis_device/Dynamixel.h"
#include "robotis_framework_common/Singleton.h"
namespace ROBOTIS
{

3
robotis_framework_common/include/robotis_framework_common/SensorModule.h
View File

@@ -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<std::string, Dynamixel *> dxls) = 0;
virtual void Process(std::map<std::string, Dynamixel *> dxls, std::map<std::string, Sensor *> sensors) = 0;
};
}

49
robotis_framework_common/include/robotis_framework_common/Singleton.h Normal file
View File

@@ -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 T>
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 <class T> T* Singleton<T>::unique_instance_ = NULL;
}
#endif /* ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ */