segway_rmp_node.cpp
Go to the documentation of this file.
00001 /*
00002  * The MIT License (MIT)
00003  * Copyright (c) 2011 William Woodall <wjwwood@gmail.com>
00004  * 
00005  * Permission is hereby granted, free of charge, to any person obtaining a 
00006  * copy of this software and associated documentation files (the "Software"), 
00007  * to deal in the Software without restriction, including without limitation 
00008  * the rights to use, copy, modify, merge, publish, distribute, sublicense, 
00009  * and/or sell copies of the Software, and to permit persons to whom the 
00010  * Software is furnished to do so, subject to the following conditions:
00011  * 
00012  * The above copyright notice and this permission notice shall be included 
00013  * in all copies or substantial portions of the Software.
00014  * 
00015  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
00016  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 
00017  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 
00018  * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 
00019  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 
00020  * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 
00021  * DEALINGS IN THE SOFTWARE.
00022  */
00023 
00024 #include <iostream>
00025 #include <sstream>
00026 #include <cmath>
00027 
00028 #include "ros/ros.h"
00029 #include <tf/transform_broadcaster.h>
00030 #include "geometry_msgs/Twist.h"
00031 #include "nav_msgs/Odometry.h"
00032 #include "segway_rmpX/SegwayStatusStamped.h"
00033 
00034 #include "segwayrmp/segwayrmp.h"
00035 
00036 #include <boost/thread.hpp>
00037 
00038 class SegwayRMPNode;
00039 
00040 static SegwayRMPNode * segwayrmp_node_instance;
00041 
00042 static double radians_to_degrees = 180.0 / M_PI;
00043 static double degrees_to_radians = M_PI / 180.0;
00044 
00045 // Message Wrappers
00046 void handleDebugMessages(const std::string &msg) {ROS_DEBUG("%s",msg.c_str());}
00047 void handleInfoMessages(const std::string &msg) {ROS_INFO("%s",msg.c_str());}
00048 void handleErrorMessages(const std::string &msg) {ROS_ERROR("%s",msg.c_str());}
00049 
00050 void handleStatusWrapper(segwayrmp::SegwayStatus::Ptr &ss);
00051 
00052 // ROS Node class
00053 class SegwayRMPNode {
00054 public:
00055     SegwayRMPNode() {
00056         n = new ros::NodeHandle("~");
00057         this->segway_rmp = NULL;
00058         this->first_odometry = false;
00059         this->last_forward_displacement = 0.0;
00060         this->last_yaw_displacement = 0.0;
00061         this->odometry_x = 0.0;
00062         this->odometry_y = 0.0;
00063         this->odometry_w = 0.0;
00064         this->count = 0;
00065     }
00066     
00067     ~SegwayRMPNode() {
00068         this->disconnect();
00069     }
00070     
00071     void disconnect() {
00072         if (this->segway_rmp != NULL)
00073             delete this->segway_rmp;
00074         this->segway_rmp = NULL;
00075     }
00076     
00077     void run() {
00078         if (this->getParameters()) {
00079             return;
00080         }
00081         
00082         this->setupSegwayRMP();
00083         
00084         this->setupROSComms();
00085         
00086         // Setup keep alive timer
00087         this->keep_alive_timer = n->createTimer(ros::Duration(1.0/20.0), &SegwayRMPNode::keepAliveCallback, this);
00088         
00089         ros::AsyncSpinner spinner(1);
00090         spinner.start();
00091         
00092         this->connected = false;
00093         while (ros::ok()) {
00094             try {
00095                 this->segway_rmp->connect();
00096                 this->connected = true;
00097             } catch (std::exception& e) {
00098                 std::string e_msg(e.what());
00099                 ROS_ERROR("Exception while connecting to the SegwayRMP, check your cables and power buttons.");
00100                 ROS_ERROR("    %s", e_msg.c_str());
00101                 this->connected = false;
00102             }
00103             if (this->spin()) { // ROS is OK, but we aren't connected, wait then try again
00104                 ROS_WARN("Not connected to the SegwayRMP, will retry in 5 seconds...");
00105                 ros::Duration(5).sleep();
00106             }
00107         }
00108     }
00109     
00110     bool spin() {
00111         if (ros::ok() && this->connected) {
00112             ROS_INFO("Segway RMP Ready.");
00113             while (ros::ok() && this->connected) {
00114                 ros::Duration(1.0).sleep();
00115             }
00116         }
00117         if (ros::ok()) { // Error not shutdown
00118             return true;
00119         } else {         // Shutdown
00120             return false;
00121         }
00122     }
00123     
00128     void keepAliveCallback(const ros::TimerEvent& e) {
00129         if (ros::ok()) {
00130             boost::mutex::scoped_lock lock(this->m_mutex);
00131 
00132             // Update the linear velocity based on the linear acceleration limits
00133             if (this->linear_vel < this->target_linear_vel) {
00134                 // Must increase linear speed
00135                 if (this->linear_pos_accel_limit == 0.0 
00136                     || this->target_linear_vel - this->linear_vel < this->linear_pos_accel_limit)
00137                     this->linear_vel = this->target_linear_vel;
00138                 else
00139                      this->linear_vel += this->linear_pos_accel_limit; 
00140             } else if (this->linear_vel > this->target_linear_vel) {
00141                 // Must decrease linear speed
00142                 if (this->linear_neg_accel_limit == 0.0 
00143                     || this->linear_vel - this->target_linear_vel < this->linear_neg_accel_limit)
00144                     this->linear_vel = this->target_linear_vel;
00145                 else
00146                      this->linear_vel -= this->linear_neg_accel_limit; 
00147             }
00148 
00149             // Update the angular velocity based on the angular acceleration limits
00150             if (this->angular_vel < this->target_angular_vel) {
00151                 // Must increase angular speed
00152                 if (this->angular_pos_accel_limit == 0.0
00153                     || this->target_angular_vel - this->angular_vel < this->angular_pos_accel_limit)
00154                     this->angular_vel = this->target_angular_vel;
00155                 else
00156                      this->angular_vel += this->angular_pos_accel_limit; 
00157             } else if (this->angular_vel > this->target_angular_vel) {
00158                 // Must decrease angular speed
00159                 if (this->angular_neg_accel_limit == 0.0 
00160                     || this->angular_vel - this->target_angular_vel < this->angular_neg_accel_limit)
00161                     this->angular_vel = this->target_angular_vel;
00162                 else
00163                      this->angular_vel -= this->angular_neg_accel_limit; 
00164             }
00165 
00166             ROS_DEBUG("Sending move command: linear velocity = %f, angular velocity = %f", 
00167                this->linear_vel, this->angular_vel);
00168 
00169             //if (this->linear_vel == 0 || this->angular_vel == 0) {
00170             //    ROS_INFO("Sending Segway Command: l=%f a=%f", this->linear_vel, this->angular_vel);
00171             //}
00172             try {
00173                 this->segway_rmp->move(this->linear_vel, this->angular_vel);
00174             } catch (std::exception& e) {
00175                 std::string e_msg(e.what());
00176                 ROS_ERROR("Error commanding Segway RMP: %s", e_msg.c_str());
00177                 this->connected = false;
00178                 this->disconnect();
00179             }
00180         }
00181     }
00182     
00183     void handleStatus(segwayrmp::SegwayStatus::Ptr &ss_ptr) {
00184         if (!this->connected)
00185             return;
00186         // Get the time
00187         ros::Time current_time = ros::Time::now();
00188         
00189         this->sss_msg.header.stamp = current_time;
00190         
00191         segwayrmp::SegwayStatus &ss = *(ss_ptr);
00192 
00193         this->sss_msg.segway.pitch_angle = ss.pitch * degrees_to_radians;
00194         this->sss_msg.segway.pitch_rate = ss.pitch_rate * degrees_to_radians;
00195         this->sss_msg.segway.roll_angle = ss.roll * degrees_to_radians;
00196         this->sss_msg.segway.roll_rate = ss.roll_rate * degrees_to_radians;
00197         this->sss_msg.segway.left_wheel_velocity = ss.left_wheel_speed;
00198         this->sss_msg.segway.right_wheel_velocity = ss.right_wheel_speed;
00199         this->sss_msg.segway.yaw_rate = ss.yaw_rate * degrees_to_radians;
00200         this->sss_msg.segway.servo_frames = ss.servo_frames;
00201         this->sss_msg.segway.left_wheel_displacement = ss.integrated_left_wheel_position;
00202         this->sss_msg.segway.right_wheel_displacement = ss.integrated_right_wheel_position;
00203         this->sss_msg.segway.forward_displacement = ss.integrated_forward_position;
00204         this->sss_msg.segway.yaw_displacement = ss.integrated_turn_position * degrees_to_radians;
00205         this->sss_msg.segway.left_motor_torque = ss.left_motor_torque;
00206         this->sss_msg.segway.right_motor_torque = ss.right_motor_torque;
00207         this->sss_msg.segway.operation_mode = ss.operational_mode;
00208         this->sss_msg.segway.gain_schedule = ss.controller_gain_schedule;
00209         this->sss_msg.segway.ui_battery = ss.ui_battery_voltage;
00210         this->sss_msg.segway.powerbase_battery = ss.powerbase_battery_voltage;
00211         this->sss_msg.segway.motors_enabled = (bool)(ss.motor_status);
00212         
00213         segway_status_pub.publish(this->sss_msg);
00214         
00215         // TODO: possibly spin this off in another thread
00216         
00217         // Grab the newest Segway data
00218         float forward_displacement = ss.integrated_forward_position;
00219         float yaw_displacement = ss.integrated_turn_position * degrees_to_radians;
00220         float yaw_rate = ss.yaw_rate * degrees_to_radians;
00221         
00222         // Integrate the displacements over time
00223         // If not the first odometry calculate the delta in displacements
00224         float vel_x = 0.0;
00225         float vel_y = 0.0;
00226         if(!this->first_odometry) {
00227             float delta_forward_displacement = 
00228                 forward_displacement - this->last_forward_displacement;
00229             double delta_time = (current_time-this->last_time).toSec();
00230             // Update accumulated odometries and calculate the x and y components 
00231             // of velocity
00232             this->odometry_w = yaw_displacement;
00233             float new_odometry_x = 
00234                 delta_forward_displacement * std::cos(this->odometry_w);
00235             vel_x = (new_odometry_x - this->odometry_x)/delta_time;
00236             this->odometry_x += new_odometry_x;
00237             float new_odometry_y = 
00238                 delta_forward_displacement * std::sin(this->odometry_w);
00239             vel_y = (new_odometry_y - this->odometry_y)/delta_time;
00240             this->odometry_y += new_odometry_y;
00241         } else {
00242             this->first_odometry = false;
00243         }
00244         // No matter what update the previouse (last) displacements
00245         this->last_forward_displacement = forward_displacement;
00246         this->last_yaw_displacement = yaw_displacement;
00247         this->last_time = current_time;
00248         
00249         // Create a Quaternion from the yaw displacement
00250         geometry_msgs::Quaternion quat = 
00251             tf::createQuaternionMsgFromYaw(yaw_displacement);
00252         
00253         // Publish the Transform odom->base_link
00254         if (this->broadcast_tf) {
00255             this->odom_trans.header.stamp = current_time;
00256             
00257             this->odom_trans.transform.translation.x = this->odometry_x;
00258             this->odom_trans.transform.translation.y = this->odometry_y;
00259             this->odom_trans.transform.translation.z = 0.0;
00260             this->odom_trans.transform.rotation = quat;
00261             
00262             //send the transform
00263             this->odom_broadcaster.sendTransform(this->odom_trans);
00264         }
00265         
00266         // Publish Odometry
00267         this->odom_msg.header.stamp = current_time;
00268         this->odom_msg.pose.pose.position.x = this->odometry_x;
00269         this->odom_msg.pose.pose.position.y = this->odometry_y;
00270         this->odom_msg.pose.pose.position.z = 0.0;
00271         this->odom_msg.pose.pose.orientation = quat;
00272         this->odom_msg.pose.covariance[0] = 0.00001;
00273         this->odom_msg.pose.covariance[7] = 0.00001;
00274         this->odom_msg.pose.covariance[14] = 1000000000000.0;
00275         this->odom_msg.pose.covariance[21] = 1000000000000.0;
00276         this->odom_msg.pose.covariance[28] = 1000000000000.0;
00277         this->odom_msg.pose.covariance[35] = 0.001;
00278         
00279         this->odom_msg.twist.twist.linear.x = vel_x;
00280         this->odom_msg.twist.twist.linear.y = vel_y;
00281         this->odom_msg.twist.twist.angular.z = yaw_rate;
00282         
00283         this->odom_pub.publish(this->odom_msg);
00284     }
00285     
00291     void motor_timeoutCallback(const ros::TimerEvent& e) {
00292         boost::mutex::scoped_lock lock(m_mutex);
00293         //ROS_INFO("Motor command timeout!  Setting target linear and angular velocities to be zero.");
00294         this->target_linear_vel = 0.0;
00295         this->target_angular_vel = 0.0;
00296     }
00297     
00301     void cmd_velCallback(const geometry_msgs::Twist::ConstPtr& msg) {
00302         if (!this->connected)
00303             return;
00304         boost::mutex::scoped_lock lock(m_mutex);
00305         double x = msg->linear.x, z = msg->angular.z;
00306         if (this->invert_x) {
00307             x *= -1;
00308         }
00309         if (this->invert_z) {
00310             z *= -1;
00311         }
00312         this->target_linear_vel = x;
00313         this->target_angular_vel = z * radians_to_degrees; // Convert to degrees
00314 
00315         //ROS_INFO("Received motor command linear vel = %f, angular vel = %f.",
00316         //    this->target_linear_vel, this->target_angular_vel);
00317 
00318         this->motor_timeout_timer = 
00319             this->n->createTimer(
00320                 ros::Duration(this->segway_motor_timeout),
00321                 &SegwayRMPNode::motor_timeoutCallback, 
00322                 this, 
00323                 true);
00324     }
00325 private:
00326     // Functions
00327     void setupROSComms() {
00328         // Subscribe to command velocities
00329         this->cmd_velSubscriber = n->subscribe("cmd_vel", 1000, &SegwayRMPNode::cmd_velCallback, this);
00330         // Advertise the SegwayStatusStamped
00331         this->segway_status_pub = n->advertise<segway_rmpX::SegwayStatusStamped>("segway_status", 1000);
00332         // Advertise the Odometry Msg
00333         this->odom_pub = n->advertise<nav_msgs::Odometry>("odom", 50);
00334     }
00335     
00336     void setupSegwayRMP() {
00337         std::stringstream ss;
00338         ss << "Connecting to Segway RMP via ";
00339         this->segway_rmp = new segwayrmp::SegwayRMP(this->interface_type, this->segway_rmp_type);
00340         if (this->interface_type_str == "serial") {
00341             ss << "serial on serial port: " << this->serial_port;
00342             this->segway_rmp->configureSerial(this->serial_port);
00343         } else if (this->interface_type_str == "usb") {
00344             ss << "usb ";
00345             if (this->usb_selector == "serial_number") {
00346                 ss << "identified by the device serial number: " << this->serial_number;
00347                 this->segway_rmp->configureUSBBySerial(this->serial_number);
00348             }
00349             if (this->usb_selector == "description") {
00350                 ss << "identified by the device description: " << this->usb_description;
00351                 this->segway_rmp->configureUSBByDescription(this->usb_description);
00352             }
00353             if (this->usb_selector == "index") {
00354                 ss << "identified by the device index: " << this->usb_index;
00355                 this->segway_rmp->configureUSBByIndex(this->usb_index);
00356             }
00357         }
00358         ROS_INFO("%s", ss.str().c_str());
00359         
00360         // Set the instance variable
00361         segwayrmp_node_instance = this;
00362         
00363         // Set callbacks for segway data and messages
00364         this->segway_rmp->setStatusCallback(handleStatusWrapper);
00365         this->segway_rmp->setLogMsgCallback("debug", handleDebugMessages);
00366         this->segway_rmp->setLogMsgCallback("info", handleInfoMessages);
00367         this->segway_rmp->setLogMsgCallback("error", handleErrorMessages);
00368     }
00369     
00370     int getParameters() {
00371         // Get Interface Type
00372         n->param("interface_type", this->interface_type_str, std::string("serial"));
00373         // Get Configurations based on Interface Type
00374         if (this->interface_type_str == "serial") {
00375             this->interface_type = segwayrmp::serial;
00376             n->param("serial_port", this->serial_port, std::string("/dev/ttyUSB0"));
00377         } else if (this->interface_type_str == "usb") {
00378             this->interface_type = segwayrmp::usb;
00379             n->param("usb_selector", this->usb_selector, std::string("index"));
00380             if (this->usb_selector == "index") {
00381                 n->param("usb_index", this->usb_index, 0);
00382             } else if (this->usb_selector == "serial_number") {
00383                 n->param("usb_serial_number", this->serial_number, std::string("00000000"));
00384                 if (this->serial_number == std::string("00000000")) {
00385                     ROS_WARN("The serial_number parameter is set to the default 00000000, which shouldn't work.");
00386                 }
00387             } else if (this->usb_selector == "description") {
00388                 n->param("usb_description", this->serial_number, std::string("Robotic Mobile Platform"));
00389             } else {
00390                 ROS_ERROR(
00391                     "Invalid USB selector: %s, valid types are 'index', 'serial_number', and 'description'.", 
00392                     this->usb_selector.c_str());
00393                 return 1;
00394             }
00395         } else {
00396             ROS_ERROR(
00397                 "Invalid interface type: %s, valid interface types are 'serial' and 'usb'.",
00398                 this->interface_type_str.c_str());
00399             return 1;
00400         }
00401         // Get Setup Motor Timeout
00402         n->param("motor_timeout", this->segway_motor_timeout, 0.5);
00403         // Get frame id parameter
00404         n->param("frame_id", frame_id, std::string("base_link"));
00405         this->sss_msg.header.frame_id = this->frame_id;
00406         this->odom_trans.header.frame_id = "odom";
00407         this->odom_trans.child_frame_id = this->frame_id;
00408         this->odom_msg.header.frame_id = "odom";
00409         this->odom_msg.child_frame_id = this->frame_id;
00410         // Get cmd_vel inversion parameters
00411         n->param("invert_linear_vel_cmds", invert_x, false);
00412         n->param("invert_angular_vel_cmds", invert_z, false);
00413         // Get option for enable/disable tf broadcasting
00414         n->param("broadcast_tf", this->broadcast_tf, true);
00415         // Get the segway rmp type
00416         std::string segway_rmp_type_str;
00417         n->param("rmp_type", segway_rmp_type_str, std::string("200/400"));
00418         if (segway_rmp_type_str == "200/400") {
00419             this->segway_rmp_type = segwayrmp::rmp200;
00420         } else if (segway_rmp_type_str == "50/100") {
00421             this->segway_rmp_type = segwayrmp::rmp100;
00422         } else {
00423             ROS_ERROR(
00424                 "Invalid rmp type: %s, valid rmp types are '200/400' and '50/100'.",
00425                 segway_rmp_type_str.c_str());
00426             return 1;
00427         }
00428 
00429         // Get the linear acceleration limits in m/s^2.  Zero means infinite.
00430         n->param("linear_pos_accel_limit", this->linear_pos_accel_limit, 0.0);
00431         n->param("linear_neg_accel_limit", this->linear_neg_accel_limit, 0.0);
00432 
00433         // Get the angular acceleration limits in deg/s^2.  Zero means infinite.
00434         n->param("angular_pos_accel_limit", this->angular_pos_accel_limit, 0.0);
00435         n->param("angular_neg_accel_limit", this->angular_neg_accel_limit, 0.0);
00436         
00437         // Check for valid acceleration limits
00438         if (this->linear_pos_accel_limit < 0) {
00439             ROS_ERROR("Invalid linear positive acceleration limit of %f (must be non-negative).",
00440                 this->linear_pos_accel_limit);
00441             return 1;
00442         }
00443         if (this->linear_neg_accel_limit < 0) {
00444             ROS_ERROR("Invalid linear negative acceleration limit of %f (must be non-negative).",
00445                 this->linear_neg_accel_limit);
00446             return 1;
00447         }
00448         if (this->angular_pos_accel_limit < 0) {
00449             ROS_ERROR("Invalid angular positive acceleration limit of %f (must be non-negative).",
00450                 this->angular_pos_accel_limit);
00451             return 1;
00452         }
00453         if (this->angular_neg_accel_limit < 0) {
00454             ROS_ERROR("Invalid angular negative acceleration limit of %f (must be non-negative).",
00455                 this->angular_neg_accel_limit);
00456             return 1;
00457         }
00458 
00459         ROS_INFO("Accel limits: linear: pos = %f, neg = %f, angular: pos = %f, neg = %f.",
00460             this->linear_pos_accel_limit, this->linear_neg_accel_limit, 
00461             this->angular_pos_accel_limit, this->angular_neg_accel_limit);
00462         
00463         // Convert the linear acceleration limits to have units of (m/s^2)/20 since
00464         // the movement commands are sent to the Segway at 20Hz.
00465         this->linear_pos_accel_limit /= 20;
00466         this->linear_neg_accel_limit /= 20;
00467 
00468         // Convert the angular acceleration limits to have units of (deg/s^2)/20 since
00469         // the movement commands are sent to the Segway at 20Hz.
00470         this->angular_pos_accel_limit /= 20;
00471         this->angular_neg_accel_limit /= 20;
00472     
00473         return 0;
00474     }
00475     
00476     // Variables
00477     ros::NodeHandle * n;
00478     
00479     ros::Timer keep_alive_timer;
00480     
00481     ros::Subscriber cmd_velSubscriber;
00482     ros::Publisher segway_status_pub;
00483     ros::Publisher odom_pub;
00484     tf::TransformBroadcaster odom_broadcaster;
00485     
00486     segwayrmp::SegwayRMP * segway_rmp;
00487     
00488     std::string interface_type_str;
00489     segwayrmp::InterfaceType interface_type;
00490     segwayrmp::SegwayRMPType segway_rmp_type;
00491     std::string serial_port;
00492     std::string usb_selector;
00493     std::string serial_number;
00494     std::string usb_description;
00495     int usb_index;
00496     
00497     double segway_motor_timeout;
00498     ros::Timer motor_timeout_timer;
00499     
00500     std::string frame_id;
00501     bool invert_x, invert_z;
00502     bool broadcast_tf;
00503     
00504     double linear_vel;
00505     double angular_vel; // The angular velocity in deg/s
00506 
00507     double target_linear_vel;  // The ideal linear velocity in m/s
00508     double target_angular_vel; // The ideal angular velocity in deg/s
00509 
00510     double linear_pos_accel_limit;  // The max linear acceleration in (m/s^2)/20
00511     double linear_neg_accel_limit;  // The max linear deceleration in (m/s^2)/20
00512     double angular_pos_accel_limit; // The max angular acceleration in (deg/s^2)/20
00513     double angular_neg_accel_limit; // The max angular deceleration in (deg/s^2)/20
00514     
00515     bool connected;
00516     
00517     segway_rmpX::SegwayStatusStamped sss_msg;
00518     geometry_msgs::TransformStamped odom_trans;
00519     nav_msgs::Odometry odom_msg;
00520     
00521     int count;
00522     
00523     bool first_odometry;
00524     float last_forward_displacement;
00525     float last_yaw_displacement;
00526     float odometry_x;
00527     float odometry_y;
00528     float odometry_w;
00529     ros::Time last_time;
00530     
00531     boost::mutex m_mutex;
00532 };
00533 
00534 // Callback wrapper
00535 void handleStatusWrapper(segwayrmp::SegwayStatus::Ptr &ss) {
00536     segwayrmp_node_instance->handleStatus(ss);
00537 }
00538 
00539 int main(int argc, char **argv) {
00540     ros::init(argc, argv, "segway_rmp_node");
00541     
00542     SegwayRMPNode segwayrmp_node;
00543     
00544     segwayrmp_node.run();
00545     
00546     return 0;
00547 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


segway_rmpX
Author(s): William Woodall
autogenerated on Thu May 2 2013 12:29:20