robotis_controller.h
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright 2018 ROBOTIS CO., LTD.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *******************************************************************************/
16 
17 /*
18  * robotis_controller.h
19  *
20  * Created on: 2016. 1. 15.
21  * Author: zerom
22  */
23 
24 #ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_
25 #define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_
26 
27 
28 #include <ros/ros.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>
34 
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"
41 
42 #include "robotis_device/robot.h"
47 
48 namespace robotis_framework
49 {
50 
52 {
55 };
56 
57 class RobotisController : public Singleton<RobotisController>
58 {
59 private:
60  boost::thread queue_thread_;
61  boost::thread gazebo_thread_;
62  boost::thread set_module_thread_;
63  boost::mutex queue_mutex_;
64 
68  pthread_t timer_thread_;
70 
71  std::list<MotionModule *> motion_modules_;
72  std::list<SensorModule *> sensor_modules_;
73  std::vector<dynamixel::GroupSyncWrite *> direct_sync_write_;
74 
75  std::map<std::string, double> sensor_result_;
76 
77  void gazeboTimerThread();
78  void msgQueueThread();
79  void setCtrlModuleThread(std::string ctrl_module);
80  void setJointCtrlModuleThread(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg);
81 
82  bool isTimerStopped();
83  void initializeSyncWrite();
84 
85 public:
88 
90  std::string gazebo_robot_name_;
91 
92  /* bulk read */
93  std::map<std::string, dynamixel::GroupBulkRead *> port_to_bulk_read_;
94 
95  /* sync write */
96  std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_position_;
97  std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_velocity_;
98  std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_current_;
99  std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_position_p_gain_;
100  std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_position_i_gain_;
101  std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_position_d_gain_;
102  std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_velocity_p_gain_;
103  std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_velocity_i_gain_;
104  std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_velocity_d_gain_;
105 
106  /* publisher */
110 
111  std::map<std::string, ros::Publisher> gazebo_joint_position_pub_;
112  std::map<std::string, ros::Publisher> gazebo_joint_velocity_pub_;
113  std::map<std::string, ros::Publisher> gazebo_joint_effort_pub_;
114 
115  static void *timerThread(void *param);
116 
118 
119  bool initialize(const std::string robot_file_path, const std::string init_file_path);
120  void initializeDevice(const std::string init_file_path);
121  void process();
122 
123  void addMotionModule(MotionModule *module);
124  void removeMotionModule(MotionModule *module);
125  void addSensorModule(SensorModule *module);
126  void removeSensorModule(SensorModule *module);
127 
128  void startTimer();
129  void stopTimer();
130  bool isTimerRunning();
131 
132  void setCtrlModule(std::string module_name);
133  void loadOffset(const std::string path);
134 
135  /* ROS Topic Callback Functions */
136  void writeControlTableCallback(const robotis_controller_msgs::WriteControlTable::ConstPtr &msg);
137  void syncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg);
138  void setControllerModeCallback(const std_msgs::String::ConstPtr &msg);
139  void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
140  void setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg);
141  void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg);
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);
145 
146  void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
147 
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);
150 
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);
154 
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);
157 
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);
161 
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);
164 
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);
168 
169  int regWrite (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0);
170 };
171 
172 }
173 
174 
175 #endif /* ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ */
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)
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::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)
std::map< std::string, ros::Publisher > gazebo_joint_effort_pub_
void removeMotionModule(MotionModule *module)
std::map< std::string, ros::Publisher > gazebo_joint_velocity_pub_
void addMotionModule(MotionModule *module)
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_
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_
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)
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_
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)


robotis_controller
Author(s): Zerom , Kayman , SCH
autogenerated on Mon Jun 10 2019 14:35:12