Skip to content

Commit 49ed1c9

Browse files
ROBOTIS-zeromROBOTIS-zerom
committed
- added writeControlTableCallback
1 parent 303e24c commit 49ed1c9

File tree

1 file changed

+187
-185
lines changed

1 file changed

+187
-185
lines changed

‎robotis_controller/include/robotis_controller/robotis_controller.h‎

Lines changed: 187 additions & 185 deletions
Original file line numberDiff line numberDiff line change
@@ -1,185 +1,187 @@
1-
/*******************************************************************************
2-
* Copyright (c) 2016, ROBOTIS CO., LTD.
3-
* All rights reserved.
4-
*
5-
* Redistribution and use in source and binary forms, with or without
6-
* modification, are permitted provided that the following conditions are met:
7-
*
8-
* * Redistributions of source code must retain the above copyright notice, this
9-
* list of conditions and the following disclaimer.
10-
*
11-
* * Redistributions in binary form must reproduce the above copyright notice,
12-
* this list of conditions and the following disclaimer in the documentation
13-
* and/or other materials provided with the distribution.
14-
*
15-
* * Neither the name of ROBOTIS nor the names of its
16-
* contributors may be used to endorse or promote products derived from
17-
* this software without specific prior written permission.
18-
*
19-
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20-
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21-
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22-
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
23-
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
24-
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
25-
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
26-
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
27-
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
28-
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29-
*******************************************************************************/
30-
31-
/*
32-
* robotis_controller.h
33-
*
34-
* Created on: 2016. 1. 15.
35-
* Author: zerom
36-
*/
37-
38-
#ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_
39-
#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_
40-
41-
42-
#include <ros/ros.h>
43-
#include <boost/thread.hpp>
44-
#include <yaml-cpp/yaml.h>
45-
#include <std_msgs/String.h>
46-
#include <std_msgs/Float64.h>
47-
#include <sensor_msgs/JointState.h>
48-
49-
#include "robotis_controller_msgs/SyncWriteItem.h"
50-
#include "robotis_controller_msgs/JointCtrlModule.h"
51-
#include "robotis_controller_msgs/GetJointModule.h"
52-
53-
#include "robotis_device/robot.h"
54-
#include "robotis_framework_common/motion_module.h"
55-
#include "robotis_framework_common/sensor_module.h"
56-
#include "dynamixel_sdk/group_bulk_read.h"
57-
#include "dynamixel_sdk/group_sync_write.h"
58-
59-
namespace robotis_framework
60-
{
61-
62-
enum ControllerMode
63-
{
64-
MotionModuleMode,
65-
DirectControlMode
66-
};
67-
68-
class RobotisController : public Singleton<RobotisController>
69-
{
70-
private:
71-
boost::thread queue_thread_;
72-
boost::thread gazebo_thread_;
73-
boost::thread set_module_thread_;
74-
boost::mutex queue_mutex_;
75-
76-
bool init_pose_loaded_;
77-
bool is_timer_running_;
78-
bool stop_timer_;
79-
pthread_t timer_thread_;
80-
ControllerMode controller_mode_;
81-
82-
std::list<MotionModule *> motion_modules_;
83-
std::list<SensorModule *> sensor_modules_;
84-
std::vector<dynamixel::GroupSyncWrite *> direct_sync_write_;
85-
86-
std::map<std::string, double> sensor_result_;
87-
88-
void gazeboTimerThread();
89-
void msgQueueThread();
90-
void setCtrlModuleThread(std::string ctrl_module);
91-
void setJointCtrlModuleThread(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg);
92-
93-
bool isTimerStopped();
94-
void initializeSyncWrite();
95-
96-
public:
97-
static const int CONTROL_CYCLE_MSEC = 8; // 8 msec
98-
99-
bool DEBUG_PRINT;
100-
Robot *robot_;
101-
102-
bool gazebo_mode_;
103-
std::string gazebo_robot_name_;
104-
105-
/* bulk read */
106-
std::map<std::string, dynamixel::GroupBulkRead *> port_to_bulk_read_;
107-
108-
/* sync write */
109-
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_position_;
110-
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_velocity_;
111-
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_current_;
112-
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_position_p_gain_;
113-
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_position_i_gain_;
114-
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_position_d_gain_;
115-
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_velocity_p_gain_;
116-
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_velocity_i_gain_;
117-
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_velocity_d_gain_;
118-
119-
/* publisher */
120-
ros::Publisher goal_joint_state_pub_;
121-
ros::Publisher present_joint_state_pub_;
122-
ros::Publisher current_module_pub_;
123-
124-
std::map<std::string, ros::Publisher> gazebo_joint_position_pub_;
125-
std::map<std::string, ros::Publisher> gazebo_joint_velocity_pub_;
126-
std::map<std::string, ros::Publisher> gazebo_joint_effort_pub_;
127-
128-
static void *timerThread(void *param);
129-
130-
RobotisController();
131-
132-
bool initialize(const std::string robot_file_path, const std::string init_file_path);
133-
void initializeDevice(const std::string init_file_path);
134-
void process();
135-
136-
void addMotionModule(MotionModule *module);
137-
void removeMotionModule(MotionModule *module);
138-
void addSensorModule(SensorModule *module);
139-
void removeSensorModule(SensorModule *module);
140-
141-
void startTimer();
142-
void stopTimer();
143-
bool isTimerRunning();
144-
145-
void setCtrlModule(std::string module_name);
146-
void loadOffset(const std::string path);
147-
148-
/* ROS Topic Callback Functions */
149-
void syncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg);
150-
void setControllerModeCallback(const std_msgs::String::ConstPtr &msg);
151-
void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
152-
void setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg);
153-
void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg);
154-
bool getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res);
155-
156-
void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
157-
158-
int ping (const std::string joint_name, uint8_t *error = 0);
159-
int ping (const std::string joint_name, uint16_t* model_number, uint8_t *error = 0);
160-
161-
int action (const std::string joint_name);
162-
int reboot (const std::string joint_name, uint8_t *error = 0);
163-
int factoryReset(const std::string joint_name, uint8_t option = 0, uint8_t *error = 0);
164-
165-
int read (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0);
166-
int readCtrlItem(const std::string joint_name, const std::string item_name, uint32_t *data, uint8_t *error = 0);
167-
168-
int read1Byte (const std::string joint_name, uint16_t address, uint8_t *data, uint8_t *error = 0);
169-
int read2Byte (const std::string joint_name, uint16_t address, uint16_t *data, uint8_t *error = 0);
170-
int read4Byte (const std::string joint_name, uint16_t address, uint32_t *data, uint8_t *error = 0);
171-
172-
int write (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0);
173-
int writeCtrlItem(const std::string joint_name, const std::string item_name, uint32_t data, uint8_t *error = 0);
174-
175-
int write1Byte (const std::string joint_name, uint16_t address, uint8_t data, uint8_t *error = 0);
176-
int write2Byte (const std::string joint_name, uint16_t address, uint16_t data, uint8_t *error = 0);
177-
int write4Byte (const std::string joint_name, uint16_t address, uint32_t data, uint8_t *error = 0);
178-
179-
int regWrite (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0);
180-
};
181-
182-
}
183-
184-
185-
#endif /* ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ */
1+
/*******************************************************************************
2+
* Copyright (c) 2016, ROBOTIS CO., LTD.
3+
* All rights reserved.
4+
*
5+
* Redistribution and use in source and binary forms, with or without
6+
* modification, are permitted provided that the following conditions are met:
7+
*
8+
* * Redistributions of source code must retain the above copyright notice, this
9+
* list of conditions and the following disclaimer.
10+
*
11+
* * Redistributions in binary form must reproduce the above copyright notice,
12+
* this list of conditions and the following disclaimer in the documentation
13+
* and/or other materials provided with the distribution.
14+
*
15+
* * Neither the name of ROBOTIS nor the names of its
16+
* contributors may be used to endorse or promote products derived from
17+
* this software without specific prior written permission.
18+
*
19+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20+
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21+
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22+
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
23+
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
24+
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
25+
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
26+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
27+
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
28+
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29+
*******************************************************************************/
30+
31+
/*
32+
* robotis_controller.h
33+
*
34+
* Created on: 2016. 1. 15.
35+
* Author: zerom
36+
*/
37+
38+
#ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_
39+
#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_
40+
41+
42+
#include <ros/ros.h>
43+
#include <boost/thread.hpp>
44+
#include <yaml-cpp/yaml.h>
45+
#include <std_msgs/String.h>
46+
#include <std_msgs/Float64.h>
47+
#include <sensor_msgs/JointState.h>
48+
49+
#include "robotis_controller_msgs/WriteControlTable.h"
50+
#include "robotis_controller_msgs/SyncWriteItem.h"
51+
#include "robotis_controller_msgs/JointCtrlModule.h"
52+
#include "robotis_controller_msgs/GetJointModule.h"
53+
54+
#include "robotis_device/robot.h"
55+
#include "robotis_framework_common/motion_module.h"
56+
#include "robotis_framework_common/sensor_module.h"
57+
#include "dynamixel_sdk/group_bulk_read.h"
58+
#include "dynamixel_sdk/group_sync_write.h"
59+
60+
namespace robotis_framework
61+
{
62+
63+
enum ControllerMode
64+
{
65+
MotionModuleMode,
66+
DirectControlMode
67+
};
68+
69+
class RobotisController : public Singleton<RobotisController>
70+
{
71+
private:
72+
boost::thread queue_thread_;
73+
boost::thread gazebo_thread_;
74+
boost::thread set_module_thread_;
75+
boost::mutex queue_mutex_;
76+
77+
bool init_pose_loaded_;
78+
bool is_timer_running_;
79+
bool stop_timer_;
80+
pthread_t timer_thread_;
81+
ControllerMode controller_mode_;
82+
83+
std::list<MotionModule *> motion_modules_;
84+
std::list<SensorModule *> sensor_modules_;
85+
std::vector<dynamixel::GroupSyncWrite *> direct_sync_write_;
86+
87+
std::map<std::string, double> sensor_result_;
88+
89+
void gazeboTimerThread();
90+
void msgQueueThread();
91+
void setCtrlModuleThread(std::string ctrl_module);
92+
void setJointCtrlModuleThread(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg);
93+
94+
bool isTimerStopped();
95+
void initializeSyncWrite();
96+
97+
public:
98+
static const int CONTROL_CYCLE_MSEC = 8; // 8 msec
99+
100+
bool DEBUG_PRINT;
101+
Robot *robot_;
102+
103+
bool gazebo_mode_;
104+
std::string gazebo_robot_name_;
105+
106+
/* bulk read */
107+
std::map<std::string, dynamixel::GroupBulkRead *> port_to_bulk_read_;
108+
109+
/* sync write */
110+
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_position_;
111+
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_velocity_;
112+
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_current_;
113+
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_position_p_gain_;
114+
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_position_i_gain_;
115+
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_position_d_gain_;
116+
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_velocity_p_gain_;
117+
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_velocity_i_gain_;
118+
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_velocity_d_gain_;
119+
120+
/* publisher */
121+
ros::Publisher goal_joint_state_pub_;
122+
ros::Publisher present_joint_state_pub_;
123+
ros::Publisher current_module_pub_;
124+
125+
std::map<std::string, ros::Publisher> gazebo_joint_position_pub_;
126+
std::map<std::string, ros::Publisher> gazebo_joint_velocity_pub_;
127+
std::map<std::string, ros::Publisher> gazebo_joint_effort_pub_;
128+
129+
static void *timerThread(void *param);
130+
131+
RobotisController();
132+
133+
bool initialize(const std::string robot_file_path, const std::string init_file_path);
134+
void initializeDevice(const std::string init_file_path);
135+
void process();
136+
137+
void addMotionModule(MotionModule *module);
138+
void removeMotionModule(MotionModule *module);
139+
void addSensorModule(SensorModule *module);
140+
void removeSensorModule(SensorModule *module);
141+
142+
void startTimer();
143+
void stopTimer();
144+
bool isTimerRunning();
145+
146+
void setCtrlModule(std::string module_name);
147+
void loadOffset(const std::string path);
148+
149+
/* ROS Topic Callback Functions */
150+
void writeControlTableCallback(const robotis_controller_msgs::WriteControlTable::ConstPtr &msg);
151+
void syncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg);
152+
void setControllerModeCallback(const std_msgs::String::ConstPtr &msg);
153+
void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
154+
void setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg);
155+
void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg);
156+
bool getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res);
157+
158+
void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
159+
160+
int ping (const std::string joint_name, uint8_t *error = 0);
161+
int ping (const std::string joint_name, uint16_t* model_number, uint8_t *error = 0);
162+
163+
int action (const std::string joint_name);
164+
int reboot (const std::string joint_name, uint8_t *error = 0);
165+
int factoryReset(const std::string joint_name, uint8_t option = 0, uint8_t *error = 0);
166+
167+
int read (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0);
168+
int readCtrlItem(const std::string joint_name, const std::string item_name, uint32_t *data, uint8_t *error = 0);
169+
170+
int read1Byte (const std::string joint_name, uint16_t address, uint8_t *data, uint8_t *error = 0);
171+
int read2Byte (const std::string joint_name, uint16_t address, uint16_t *data, uint8_t *error = 0);
172+
int read4Byte (const std::string joint_name, uint16_t address, uint32_t *data, uint8_t *error = 0);
173+
174+
int write (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0);
175+
int writeCtrlItem(const std::string joint_name, const std::string item_name, uint32_t data, uint8_t *error = 0);
176+
177+
int write1Byte (const std::string joint_name, uint16_t address, uint8_t data, uint8_t *error = 0);
178+
int write2Byte (const std::string joint_name, uint16_t address, uint16_t data, uint8_t *error = 0);
179+
int write4Byte (const std::string joint_name, uint16_t address, uint32_t data, uint8_t *error = 0);
180+
181+
int regWrite (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0);
182+
};
183+
184+
}
185+
186+
187+
#endif /* ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ */

0 commit comments

Comments
 (0)