-
Notifications
You must be signed in to change notification settings - Fork 0
/
general_velocity_control.cpp
198 lines (153 loc) · 6.13 KB
/
general_velocity_control.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
#include "dynamixel_workbench_controllers/general_velocity_control.h"
#include <vector>
#include <sstream>
VelocityControl::VelocityControl()
:node_handle_(""),
dxl_cnt_(0)
{
std::string device_name = node_handle_.param<std::string>("device_name", "/dev/ttyUSB0");
uint32_t dxl_baud_rate = node_handle_.param<int>("baud_rate", 4000000);
std::string str = node_handle_.param<std::string>("id_list", "1");
ROS_INFO("IDs provided: %s", str.c_str());
std::vector<int> vect;
std::stringstream ss(str);
int i;
while (ss >> i)
{
vect.push_back(i);
if (ss.peek() == ',' || ss.peek() == ' ')
ss.ignore();
}
dxl_cnt_ = vect.size();
ROS_INFO("number of dxls provided: %d", dxl_cnt_);
uint32_t profile_velocity = node_handle_.param<int>("profile_velocity", 200);
uint32_t profile_acceleration = node_handle_.param<int>("profile_acceleration", 50);
dxl_id_ = new uint8_t[dxl_cnt_];
for (int id_index=0; id_index < dxl_cnt_; id_index++)
{
dxl_id_[id_index] = vect[id_index];
}
dxl_wb_ = new DynamixelWorkbench;
dxl_wb_->begin(device_name.c_str(), dxl_baud_rate);
for (int index = 0; index < dxl_cnt_; index++)
{
uint16_t get_model_number;
if (dxl_wb_->ping(dxl_id_[index], &get_model_number) != true)
{
ROS_ERROR("Could not find motors, please check id and baud rate");
ros::shutdown();
return;
}
}
initMsg();
for (int index = 0; index < dxl_cnt_; index++)
dxl_wb_->wheelMode(dxl_id_[index], profile_velocity, profile_acceleration);
dxl_wb_->addSyncWrite("Goal_Velocity");
initPublisher();
initSubscriber();
}
VelocityControl::~VelocityControl()
{
for (int index = 0; index < 2; index++)
dxl_wb_->itemWrite(dxl_id_[index], "Torque_Enable", 0);
ros::shutdown();
}
void VelocityControl::initMsg()
{
printf("-----------------------------------------------------------------------\n");
printf(" dynamixel_workbench controller; velocity control \n");
printf(" -This code supports MX2.0 and X Series- \n");
printf("-----------------------------------------------------------------------\n");
printf("\n");
for (int index = 0; index < dxl_cnt_; index++)
{
printf("MODEL : %s\n", dxl_wb_->getModelName(dxl_id_[index]));
printf("ID : %d\n", dxl_id_[index]);
printf("\n");
}
printf("-----------------------------------------------------------------------\n");
}
void VelocityControl::initPublisher()
{
dynamixel_state_list_pub_ = node_handle_.advertise<dynamixel_workbench_msgs::DynamixelStateList>("dynamixel_state", 10);
joint_states_pub_ = node_handle_.advertise<sensor_msgs::JointState>("joint_states", 10);
}
void VelocityControl::initSubscriber()
{
cmd_vel_sub_ = node_handle_.subscribe("cmd_joint_vel", 10, &VelocityControl::commandVelocityCallback, this);
}
void VelocityControl::dynamixelStatePublish()
{
dynamixel_workbench_msgs::DynamixelState dynamixel_state[dxl_cnt_];
dynamixel_workbench_msgs::DynamixelStateList dynamixel_state_list;
for (int index = 0; index < dxl_cnt_; index++)
{
dynamixel_state[index].model_name = std::string(dxl_wb_->getModelName(dxl_id_[index]));
dynamixel_state[index].id = dxl_id_[index];
dynamixel_state[index].torque_enable = dxl_wb_->itemRead(dxl_id_[index], "Torque_Enable");
dynamixel_state[index].present_position = dxl_wb_->itemRead(dxl_id_[index], "Present_Position");
dynamixel_state[index].present_velocity = dxl_wb_->itemRead(dxl_id_[index], "Present_Velocity");
dynamixel_state[index].present_current = dxl_wb_->itemRead(dxl_id_[index], "Present_Current");
dynamixel_state[index].goal_position = dxl_wb_->itemRead(dxl_id_[index], "Goal_Position");
dynamixel_state[index].goal_velocity = dxl_wb_->itemRead(dxl_id_[index], "Goal_Velocity");
dynamixel_state[index].moving = dxl_wb_->itemRead(dxl_id_[index], "Moving");
dynamixel_state_list.dynamixel_state.push_back(dynamixel_state[index]);
}
dynamixel_state_list_pub_.publish(dynamixel_state_list);
}
void VelocityControl::jointStatePublish()
{
int32_t present_position[dxl_cnt_] = {0, };
for (int index = 0; index < dxl_cnt_; index++)
present_position[index] = dxl_wb_->itemRead(dxl_id_[index], "Present_Position");
int32_t present_velocity[dxl_cnt_] = {0, };
for (int index = 0; index < dxl_cnt_; index++)
present_velocity[index] = dxl_wb_->itemRead(dxl_id_[index], "Present_Velocity");
int16_t present_current[dxl_cnt_] = {0, };
for (int index = 0; index < dxl_cnt_; index++)
present_current[index] = dxl_wb_->itemRead(dxl_id_[index], "Present_Current");
sensor_msgs::JointState dynamixel_;
dynamixel_.header.stamp = ros::Time::now();
for (int index = 0; index < dxl_cnt_; index++)
{
std::stringstream id_num;
id_num << "id_" << (int)(dxl_id_[index]);
dynamixel_.name.push_back(id_num.str());
dynamixel_.position.push_back(dxl_wb_->convertValue2Radian(dxl_id_[index], present_position[index]));
dynamixel_.velocity.push_back(dxl_wb_->convertValue2Velocity(dxl_id_[index], present_velocity[index]));
dynamixel_.effort.push_back(dxl_wb_->convertValue2Torque(dxl_id_[index], present_current[index]));
}
joint_states_pub_.publish(dynamixel_);
}
void VelocityControl::controlLoop()
{
dynamixelStatePublish();
jointStatePublish();
}
void VelocityControl::commandVelocityCallback(const std_msgs::Float64MultiArray::ConstPtr &msg)
{
bool dxl_comm_result = false;
int32_t dynamixel_velocity[dxl_cnt_];
const float RPM_OF_DXL = 0.229;
const float VELOCITY_CONSTANT_VALUE = 1 / (RPM_OF_DXL * 0.10472); // convert to rad/s
for (int id_index=0; id_index < dxl_cnt_; id_index++)
{
float vel_ind = msg->data[id_index];
dynamixel_velocity[id_index] = vel_ind * VELOCITY_CONSTANT_VALUE;
}
dxl_wb_->syncWrite("Goal_Velocity", dynamixel_velocity);
}
int main(int argc, char **argv)
{
// Init ROS node
ros::init(argc, argv, "general_velocity_control");
VelocityControl vel_ctrl;
ros::Rate loop_rate(250);
while (ros::ok())
{
vel_ctrl.controlLoop();
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}