1 #ifndef MD49_BASE_CONTROLLER_CLASS_H_ 2 #define MD49_BASE_CONTROLLER_CLASS_H_ 18 md49_encoders_pub = n.
advertise<md49_messages::md49_encoders>(
"md49_encoders",10);
19 md49_data_pub = n.
advertise<md49_messages::md49_data>(
"md49_data",10);
23 n.
param<std::string>(
"serialport/name", serialport,
"/dev/ttyS0");
24 n.
param(
"serialport/bps", serialport_bps, 38400);
25 n.
param(
"md49/mode", initial_md49_mode, 0);
26 n.
param(
"md49/acceleration", initial_md49_acceleration, 5);
27 n.
param(
"md49/regulator", initial_md49_regulator,
true);
28 n.
param(
"md49/timeout", initial_md49_timeout,
true);
29 n.
param(
"md49/speed_l", requested_speed_l, 128);
30 n.
param(
"md49/speed_r", requested_speed_r, 128);
31 actual_speed_l=requested_speed_l;
32 actual_speed_r=requested_speed_r;
41 if (vel_cmd.linear.x != 0){
42 requested_speed_l = 128+(635*vel_cmd.linear.x);
43 requested_speed_r = 128+(635*vel_cmd.linear.x);
46 if (vel_cmd.linear.x==0 && vel_cmd.angular.z==0){
47 requested_speed_l = 128;
48 requested_speed_r = 128;
51 if (vel_cmd.angular.z != 0){
52 requested_speed_l = 128 - (127*vel_cmd.angular.z);
53 requested_speed_r = 128 + (127*vel_cmd.angular.z);
56 ROS_INFO(
"base_controller: Received /cmd_vel message. Requested speed_l=%i, speed_r=%i",requested_speed_l,requested_speed_r);
64 try{ device.open(serialport.c_str(), serialport_bps); }
65 catch(cereal::Exception& e)
67 ROS_FATAL(
"base_controller: Failed to open serialport %s!",serialport.c_str());
70 ROS_INFO(
"base_controller: Opened Serialport at %s with %i bps.",serialport.c_str(),serialport_bps);
78 md49_encoders_pub.publish(md49_encoders);
86 md49_data.speed_l=get_speed_l();
87 md49_data.speed_r=get_speed_r();
88 md49_data.volts=get_volts();
89 md49_data.current_l=get_current_l();
90 md49_data.current_r=get_current_r();
91 md49_data.acceleration=get_acceleration();
92 md49_data.mode=get_mode();
93 md49_data.error=get_error();
94 md49_data_pub.publish(md49_data);
106 void init_md49(
int speed_l,
int speed_r,
int mode,
int acceleration,
bool timeout,
bool regulator)
108 set_speed(speed_l,speed_r);
110 set_acceleration(acceleration);
115 else if (timeout==
false)
123 else if (regulator==
false)
136 const char md49_set_speed_l[]={0x00,0x31,speed_l};
137 device.write(md49_set_speed_l,3);
139 const char md49_set_speed_r[]={0x00,0x32,speed_r};
140 device.write(md49_set_speed_r,3);
149 const char md49_set_mode[]={0x00,0x34,mode};
150 device.write(md49_set_mode,3);
151 ROS_INFO(
"base_controller: Set mode=%i on MD49", mode);
159 const char md49_set_acceleration[]={0x00,0x33,acceleration};
160 device.write(md49_set_acceleration,3);
161 ROS_INFO(
"base_controller: Set acceleration=%i on MD49", acceleration);
167 const char md49_enable_timeout[] = {0x00,0x39};
168 device.write(md49_enable_timeout,2);
170 ROS_INFO(
"base_controller: Enabled timeout on MD49");
176 const char md49_enable_regulator[] = {0x00,0x37};
177 device.write(md49_enable_regulator,2);
178 md49_data.regulator=1;
179 ROS_INFO(
"base_controller: Enabled regulator on MD49");
185 const char md49_disable_timeout[] = {0x00,0x38};
186 device.write(md49_disable_timeout,2);
188 ROS_INFO(
"base_controller: Disabled timeout on MD49");
194 const char md49_disable_regulator[] = {0x00,0x36};
195 device.write(md49_disable_regulator,2);
196 md49_data.regulator=0;
197 ROS_INFO(
"base_controller: Disabled regulator on MD49");
204 const char md49_get_acceleration[] = {0x00,0x2A};
205 device.write(md49_get_acceleration,2);
206 try{ device.read(reply, 1,
TIMEOUT); }
207 catch(cereal::TimeoutException& e){
208 ROS_ERROR(
"base_controller: Timeout reading MD49 acceleration!");
218 const char md49_get_mode[] = {0x00,0x2B};
219 device.write(md49_get_mode,2);
220 try{ device.read(reply, 1,
TIMEOUT); }
221 catch(cereal::TimeoutException& e){
222 ROS_ERROR(
"base_controller: Timeout reading MD49 Mode!");
232 const char md49_get_speed_l[] = {0x00,0x21};
233 device.write(md49_get_speed_l,2);
234 try{ device.read(reply, 1,
TIMEOUT); }
235 catch(cereal::TimeoutException& e){
236 ROS_ERROR(
"base_controller: Timeout reading MD49 speed_l!");
246 const char md49_get_speed_r[] = {0x00,0x22};
247 device.write(md49_get_speed_r,2);
248 try{ device.read(reply, 1,
TIMEOUT); }
249 catch(cereal::TimeoutException& e){
250 ROS_ERROR(
"base_controller: Timeout reading MD49 speed_r!");
259 const char md49_get_encoders[] = {0x00,0x25};
260 device.write(md49_get_encoders,2);
264 try{ device.read(reply, 8,
TIMEOUT); }
265 catch(cereal::TimeoutException& e){
266 ROS_ERROR(
"base_controller: Timeout reading MD49 encodervalues!");
271 md49_encoders.encoder_l = reply[0] << 24;
272 md49_encoders.encoder_l |= (reply[1] << 16);
273 md49_encoders.encoder_l |= (reply[2] << 8);
274 md49_encoders.encoder_l |= (reply[3]);
275 md49_encoders.encoder_r = reply[4] << 24;
276 md49_encoders.encoder_r |= (reply[5] << 16);
277 md49_encoders.encoder_r |= (reply[6] << 8);
278 md49_encoders.encoder_r |= (reply[7]);
279 md49_encoders.encoderbyte1l=reply[0];
280 md49_encoders.encoderbyte2l=reply[1];
281 md49_encoders.encoderbyte3l=reply[2];
282 md49_encoders.encoderbyte4l=reply[3];
283 md49_encoders.encoderbyte1r=reply[4];
284 md49_encoders.encoderbyte2r=reply[5];
285 md49_encoders.encoderbyte3r=reply[6];
286 md49_encoders.encoderbyte4r=reply[7];
294 const char md49_get_volts[] = {0x00,0x26};
295 device.write(md49_get_volts,2);
296 try{ device.read(reply, 1,
TIMEOUT); }
297 catch(cereal::TimeoutException& e){
298 ROS_ERROR(
"base_controller: Timeout reading MD49 volts!");
308 const char md49_get_current_l[] = {0x00,0x27};
309 device.write(md49_get_current_l,2);
310 try{ device.read(reply, 1,
TIMEOUT); }
311 catch(cereal::TimeoutException& e){
312 ROS_ERROR(
"base_controller: Timeout reading MD49 current_l!");
322 const char md49_get_current_r[] = {0x00,0x28};
323 device.write(md49_get_current_r,2);
324 try{ device.read(reply, 1,
TIMEOUT); }
325 catch(cereal::TimeoutException& e){
326 ROS_ERROR(
"base_controller: Timeout reading MD49 current_r!");
337 const char md49_get_error[] = {0x00,0x2D};
338 device.write(md49_get_error,2);
339 try{ device.read(reply, 1,
TIMEOUT); }
340 catch(cereal::TimeoutException& e){
341 ROS_ERROR(
"base_controller: Timeout reading MD49 errorbyte!");
350 const char md49_reset_encoders[] = {0x00,0x35};
351 device.write(md49_reset_encoders,2);
352 ROS_INFO(
"base_controller: Reset encoders on MD49");
361 return requested_speed_l;
369 return requested_speed_r;
377 requested_speed_l=speed_l;
385 requested_speed_r=speed_r;
393 return actual_speed_l;
401 return actual_speed_r;
409 actual_speed_l=speed_l;
417 actual_speed_r=speed_r;
425 return initial_md49_mode;
432 return initial_md49_acceleration;
440 return initial_md49_timeout;
448 return initial_md49_regulator;
void set_requested_speed_r(int speed_r)
set_requested_speed_r
int get_speed_l()
This function reads left drives speed from MD49.
bool initial_md49_regulator
int get_current_l()
This function reads left drives current from MD49.
void cmd_vel_callback(const geometry_msgs::Twist &vel_cmd)
This is the callback-function for topic /cmd_vel.
BaseController()
Constructor for class BaseController.
int get_initial_md49_timeout()
get_initial_md49_timeout
void disable_timeout(void)
This function disables Timeout on MD49.
int get_mode()
This function reads the mode that is set from MD49.
void disable_regulator(void)
This function disables regulator on MD49.
int get_actual_speed_r()
get_actual_speed_r
ros::Publisher md49_data_pub
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void reset_encoders(void)
This function resets encoders on MD49.
void publish_md49_data()
This function reads data and parameters from MD49 and publishes them as topic /md49_data.
void set_mode(int mode)
This function sets mode on MD49.
void publish_encoders()
This function reads encodervalues from MD49 and publishes them as topic /md49_encoders.
void enable_timeout(void)
This function enables timeout on MD49.
bool initial_md49_timeout
void init_md49(int speed_l, int speed_r, int mode, int acceleration, bool timeout, bool regulator)
This function sets parameters for MD49 as read from config file or as set as defaults.
int get_acceleration()
This function reads the acceleration that is set from MD49.
int get_initial_md49_mode()
get_initial_md49_mode
int get_actual_speed_l()
get_actual_speed_l
int get_speed_r()
This function reads right drives speed from MD49.
int get_current_r()
This function reads right drives current from MD49.
int get_requested_speed_l()
get_requested_speed_l
cereal::CerealPort device
void set_actual_speed_r(int speed_r)
set_actual_speed_r
int get_initial_md49_regulator()
get_initial_md49_regulator
int get_error()
This function reads error code from MD49.
void open_serialport()
This function opens serial port MD49 is connected to.
int get_requested_speed_r()
get_requested_speed_r
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
md49_messages::md49_encoders md49_encoders
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Subscriber sub_cmd_vel
void set_requested_speed_l(int speed_l)
set_requested_speed_l
ros::Publisher md49_encoders_pub
void set_speed(int speed_l, int speed_r)
This function sets speed for left and right drive on MD49.
int get_initial_md49_acceleration()
get_initial_md49_acceleration
void set_actual_speed_l(int speed_l)
set_actual_speed_l
void enable_regulator(void)
This function enables regulator on MD49.
void set_acceleration(int acceleration)
This function sets acceleration on MD49.
md49_messages::md49_data md49_data
int initial_md49_acceleration
void get_encoders(void)
This function reads encodervalues from MD49.
int get_volts()
This function reads supply voltage from MD49.