24 #ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ 25 #define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ 29 #include <boost/thread.hpp> 30 #include <yaml-cpp/yaml.h> 31 #include <std_msgs/String.h> 32 #include <std_msgs/Float64.h> 33 #include <sensor_msgs/JointState.h> 35 #include "robotis_controller_msgs/WriteControlTable.h" 36 #include "robotis_controller_msgs/SyncWriteItem.h" 37 #include "robotis_controller_msgs/JointCtrlModule.h" 38 #include "robotis_controller_msgs/GetJointModule.h" 39 #include "robotis_controller_msgs/SetJointModule.h" 40 #include "robotis_controller_msgs/SetModule.h" 119 bool initialize(
const std::string robot_file_path,
const std::string init_file_path);
142 bool getJointCtrlModuleService(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res);
143 bool setJointCtrlModuleService(robotis_controller_msgs::SetJointModule::Request &req, robotis_controller_msgs::SetJointModule::Response &res);
144 bool setCtrlModuleService(robotis_controller_msgs::SetModule::Request &req, robotis_controller_msgs::SetModule::Response &res);
148 int ping (
const std::string joint_name, uint8_t *error = 0);
149 int ping (
const std::string joint_name, uint16_t* model_number, uint8_t *error = 0);
151 int action (
const std::string joint_name);
152 int reboot (
const std::string joint_name, uint8_t *error = 0);
153 int factoryReset(
const std::string joint_name, uint8_t option = 0, uint8_t *error = 0);
155 int read (
const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0);
156 int readCtrlItem(
const std::string joint_name,
const std::string item_name, uint32_t *data, uint8_t *error = 0);
158 int read1Byte (
const std::string joint_name, uint16_t address, uint8_t *data, uint8_t *error = 0);
159 int read2Byte (
const std::string joint_name, uint16_t address, uint16_t *data, uint8_t *error = 0);
160 int read4Byte (
const std::string joint_name, uint16_t address, uint32_t *data, uint8_t *error = 0);
162 int write (
const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0);
163 int writeCtrlItem(
const std::string joint_name,
const std::string item_name, uint32_t data, uint8_t *error = 0);
165 int write1Byte (
const std::string joint_name, uint16_t address, uint8_t data, uint8_t *error = 0);
166 int write2Byte (
const std::string joint_name, uint16_t address, uint16_t data, uint8_t *error = 0);
167 int write4Byte (
const std::string joint_name, uint16_t address, uint32_t data, uint8_t *error = 0);
169 int regWrite (
const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0);
void setCtrlModuleThread(std::string ctrl_module)
std::list< SensorModule * > sensor_modules_
void initializeDevice(const std::string init_file_path)
int read4Byte(const std::string joint_name, uint16_t address, uint32_t *data, uint8_t *error=0)
boost::thread gazebo_thread_
int read(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)
int reboot(const std::string joint_name, uint8_t *error=0)
int write(const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error=0)
std::string gazebo_robot_name_
void initializeSyncWrite()
std::map< std::string, ros::Publisher > gazebo_joint_position_pub_
std::map< std::string, dynamixel::GroupSyncWrite * > port_to_sync_write_position_i_gain_
void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg)
ControllerMode controller_mode_
std::map< std::string, ros::Publisher > gazebo_joint_effort_pub_
boost::thread set_module_thread_
void removeMotionModule(MotionModule *module)
std::map< std::string, ros::Publisher > gazebo_joint_velocity_pub_
void addMotionModule(MotionModule *module)
ros::Publisher goal_joint_state_pub_
void setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg)
int write2Byte(const std::string joint_name, uint16_t address, uint16_t data, uint8_t *error=0)
std::map< std::string, dynamixel::GroupSyncWrite * > port_to_sync_write_velocity_d_gain_
boost::mutex queue_mutex_
std::map< std::string, double > sensor_result_
int write1Byte(const std::string joint_name, uint16_t address, uint8_t data, uint8_t *error=0)
bool setJointCtrlModuleService(robotis_controller_msgs::SetJointModule::Request &req, robotis_controller_msgs::SetJointModule::Response &res)
std::map< std::string, dynamixel::GroupSyncWrite * > port_to_sync_write_position_p_gain_
bool initialize(const std::string robot_file_path, const std::string init_file_path)
bool setCtrlModuleService(robotis_controller_msgs::SetModule::Request &req, robotis_controller_msgs::SetModule::Response &res)
void loadOffset(const std::string path)
std::map< std::string, dynamixel::GroupSyncWrite * > port_to_sync_write_current_
std::map< std::string, dynamixel::GroupSyncWrite * > port_to_sync_write_position_d_gain_
ros::Publisher current_module_pub_
std::map< std::string, dynamixel::GroupSyncWrite * > port_to_sync_write_velocity_i_gain_
std::list< MotionModule * > motion_modules_
int action(const std::string joint_name)
void setControllerModeCallback(const std_msgs::String::ConstPtr &msg)
int write4Byte(const std::string joint_name, uint16_t address, uint32_t data, uint8_t *error=0)
std::map< std::string, dynamixel::GroupSyncWrite * > port_to_sync_write_velocity_
void addSensorModule(SensorModule *module)
std::map< std::string, dynamixel::GroupBulkRead * > port_to_bulk_read_
void syncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg)
int ping(const std::string joint_name, uint8_t *error=0)
static void * timerThread(void *param)
void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg)
int read1Byte(const std::string joint_name, uint16_t address, uint8_t *data, uint8_t *error=0)
void removeSensorModule(SensorModule *module)
std::vector< dynamixel::GroupSyncWrite * > direct_sync_write_
void setJointCtrlModuleThread(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg)
ros::Publisher present_joint_state_pub_
void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg)
int writeCtrlItem(const std::string joint_name, const std::string item_name, uint32_t data, uint8_t *error=0)
bool getJointCtrlModuleService(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res)
std::map< std::string, dynamixel::GroupSyncWrite * > port_to_sync_write_position_
std::map< std::string, dynamixel::GroupSyncWrite * > port_to_sync_write_velocity_p_gain_
boost::thread queue_thread_
int factoryReset(const std::string joint_name, uint8_t option=0, uint8_t *error=0)
void writeControlTableCallback(const robotis_controller_msgs::WriteControlTable::ConstPtr &msg)
int readCtrlItem(const std::string joint_name, const std::string item_name, uint32_t *data, uint8_t *error=0)
void setCtrlModule(std::string module_name)
int read2Byte(const std::string joint_name, uint16_t address, uint16_t *data, uint8_t *error=0)