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_rmp/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 = true;
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->linear_vel = 0.0;
00065         this->angular_vel = 0.0;
00066         this->target_linear_vel = 0.0;
00067         this->target_angular_vel = 0.0;
00068         this->initial_integrated_forward_position = 0.0;
00069         this->initial_integrated_left_wheel_position = 0.0;
00070         this->initial_integrated_right_wheel_position = 0.0;
00071         this->initial_integrated_turn_position = 0.0;
00072         this->count = 0;
00073     }
00074     
00075     ~SegwayRMPNode() {
00076         this->disconnect();
00077     }
00078     
00079     void disconnect() {
00080         if (this->segway_rmp != NULL)
00081             delete this->segway_rmp;
00082         this->segway_rmp = NULL;
00083     }
00084     
00085     void run() {
00086         if (this->getParameters()) {
00087             return;
00088         }
00089         
00090         this->setupSegwayRMP();
00091         
00092         this->setupROSComms();
00093         
00094         // Setup keep alive timer
00095         this->keep_alive_timer = n->createTimer(ros::Duration(1.0/20.0), &SegwayRMPNode::keepAliveCallback, this);
00096         
00097         ros::AsyncSpinner spinner(1);
00098         spinner.start();
00099         
00100         this->odometry_reset_start_time = ros::Time::now();
00101 
00102         this->connected = false;
00103         while (ros::ok()) {
00104             try {
00105                 this->segway_rmp->connect();
00106                 this->connected = true;
00107             } catch (std::exception& e) {
00108                 std::string e_msg(e.what());
00109                 ROS_ERROR("Exception while connecting to the SegwayRMP, check your cables and power buttons.");
00110                 ROS_ERROR("    %s", e_msg.c_str());
00111                 this->connected = false;
00112             }
00113             if (this->spin()) { // ROS is OK, but we aren't connected, wait then try again
00114                 ROS_WARN("Not connected to the SegwayRMP, will retry in 5 seconds...");
00115                 ros::Duration(5).sleep();
00116             }
00117         }
00118     }
00119     
00120     bool spin() {
00121         if (ros::ok() && this->connected) {
00122             ROS_INFO("Segway RMP Ready.");
00123             while (ros::ok() && this->connected) {
00124                 ros::Duration(1.0).sleep();
00125             }
00126         }
00127         if (ros::ok()) { // Error not shutdown
00128             return true;
00129         } else {         // Shutdown
00130             return false;
00131         }
00132     }
00133     
00138     void keepAliveCallback(const ros::TimerEvent& e) {
00139 
00140         if (!this->connected || this->reset_odometry)
00141           return;
00142 
00143         if (ros::ok()) {
00144             boost::mutex::scoped_lock lock(this->m_mutex);
00145 
00146             // Update the linear velocity based on the linear acceleration limits
00147             if (this->linear_vel < this->target_linear_vel) {
00148                 // Must increase linear speed
00149                 if (this->linear_pos_accel_limit == 0.0 
00150                     || this->target_linear_vel - this->linear_vel < this->linear_pos_accel_limit)
00151                     this->linear_vel = this->target_linear_vel;
00152                 else
00153                      this->linear_vel += this->linear_pos_accel_limit; 
00154             } else if (this->linear_vel > this->target_linear_vel) {
00155                 // Must decrease linear speed
00156                 if (this->linear_neg_accel_limit == 0.0 
00157                     || this->linear_vel - this->target_linear_vel < this->linear_neg_accel_limit)
00158                     this->linear_vel = this->target_linear_vel;
00159                 else
00160                      this->linear_vel -= this->linear_neg_accel_limit; 
00161             }
00162 
00163             // Update the angular velocity based on the angular acceleration limits
00164             if (this->angular_vel < this->target_angular_vel) {
00165                 // Must increase angular speed
00166                 if (this->angular_pos_accel_limit == 0.0
00167                     || this->target_angular_vel - this->angular_vel < this->angular_pos_accel_limit)
00168                     this->angular_vel = this->target_angular_vel;
00169                 else
00170                      this->angular_vel += this->angular_pos_accel_limit; 
00171             } else if (this->angular_vel > this->target_angular_vel) {
00172                 // Must decrease angular speed
00173                 if (this->angular_neg_accel_limit == 0.0 
00174                     || this->angular_vel - this->target_angular_vel < this->angular_neg_accel_limit)
00175                     this->angular_vel = this->target_angular_vel;
00176                 else
00177                      this->angular_vel -= this->angular_neg_accel_limit; 
00178             }
00179 
00180             ROS_DEBUG("Sending move command: linear velocity = %f, angular velocity = %f", 
00181                this->linear_vel, this->angular_vel);
00182 
00183             //if (this->linear_vel == 0 || this->angular_vel == 0) {
00184             //    ROS_INFO("Sending Segway Command: l=%f a=%f", this->linear_vel, this->angular_vel);
00185             //}
00186             try {
00187                 this->segway_rmp->move(this->linear_vel, this->angular_vel);
00188             } catch (std::exception& e) {
00189                 std::string e_msg(e.what());
00190                 ROS_ERROR("Error commanding Segway RMP: %s", e_msg.c_str());
00191                 this->connected = false;
00192                 this->disconnect();
00193             }
00194         }
00195     }
00196     
00197     void handleStatus(segwayrmp::SegwayStatus::Ptr &ss_ptr) {
00198         if (!this->connected)
00199             return;
00200         // Get the time
00201         ros::Time current_time = ros::Time::now();
00202 
00203         this->sss_msg.header.stamp = current_time;
00204         
00205         segwayrmp::SegwayStatus &ss = *(ss_ptr);
00206 
00207         // Check if an odometry reset is still required
00208         if (this->reset_odometry) {
00209           if ((current_time - this->odometry_reset_start_time).toSec() < 0.25) {
00210             return; // discard readings for the first 0.25 seconds
00211           }
00212           if (fabs(ss.integrated_forward_position) < 1e-3 &&
00213               fabs(ss.integrated_turn_position) < 1e-3 &&
00214               fabs(ss.integrated_left_wheel_position) < 1e-3 &&
00215               fabs(ss.integrated_right_wheel_position) < 1e-3) {
00216             this->initial_integrated_forward_position = ss.integrated_forward_position;
00217             this->initial_integrated_left_wheel_position = ss.integrated_left_wheel_position;
00218             this->initial_integrated_right_wheel_position = ss.integrated_right_wheel_position;
00219             this->initial_integrated_turn_position = ss.integrated_turn_position;
00220             ROS_INFO("Integrators reset by Segway RMP successfully");
00221             this->reset_odometry = false;
00222           } else if ((current_time - this->odometry_reset_start_time).toSec() > this->odometry_reset_duration) {
00223             this->initial_integrated_forward_position = ss.integrated_forward_position;
00224             this->initial_integrated_left_wheel_position = ss.integrated_left_wheel_position;
00225             this->initial_integrated_right_wheel_position = ss.integrated_right_wheel_position;
00226             this->initial_integrated_turn_position = ss.integrated_turn_position;
00227             ROS_INFO("Integrator reset by Segway RMP failed. Performing software reset"); 
00228             this->reset_odometry = false;
00229           } else {
00230             return; // continue waiting for odometry to be reset
00231           }
00232         }
00233 
00234         this->sss_msg.segway.pitch_angle = ss.pitch * degrees_to_radians;
00235         this->sss_msg.segway.pitch_rate = ss.pitch_rate * degrees_to_radians;
00236         this->sss_msg.segway.roll_angle = ss.roll * degrees_to_radians;
00237         this->sss_msg.segway.roll_rate = ss.roll_rate * degrees_to_radians;
00238         this->sss_msg.segway.left_wheel_velocity = ss.left_wheel_speed;
00239         this->sss_msg.segway.right_wheel_velocity = ss.right_wheel_speed;
00240         this->sss_msg.segway.yaw_rate = ss.yaw_rate * degrees_to_radians;
00241         this->sss_msg.segway.servo_frames = ss.servo_frames;
00242         this->sss_msg.segway.left_wheel_displacement = 
00243             ss.integrated_left_wheel_position - this->initial_integrated_left_wheel_position;
00244         this->sss_msg.segway.right_wheel_displacement = 
00245             ss.integrated_right_wheel_position - this->initial_integrated_right_wheel_position;
00246         this->sss_msg.segway.forward_displacement = 
00247             ss.integrated_forward_position - this->initial_integrated_forward_position;
00248         this->sss_msg.segway.yaw_displacement = 
00249             (ss.integrated_turn_position - this->initial_integrated_turn_position) * degrees_to_radians;
00250         this->sss_msg.segway.left_motor_torque = ss.left_motor_torque;
00251         this->sss_msg.segway.right_motor_torque = ss.right_motor_torque;
00252         this->sss_msg.segway.operation_mode = ss.operational_mode;
00253         this->sss_msg.segway.gain_schedule = ss.controller_gain_schedule;
00254         this->sss_msg.segway.ui_battery = ss.ui_battery_voltage;
00255         this->sss_msg.segway.powerbase_battery = ss.powerbase_battery_voltage;
00256         this->sss_msg.segway.motors_enabled = (bool)(ss.motor_status);
00257         
00258         segway_status_pub.publish(this->sss_msg);
00259         
00260         // TODO: possibly spin this off in another thread
00261         
00262         // Grab the newest Segway data
00263         float forward_displacement = 
00264             (ss.integrated_forward_position - this->initial_integrated_forward_position) * 
00265             this->linear_odom_scale;
00266         float yaw_displacement = 
00267             (ss.integrated_turn_position - this->initial_integrated_turn_position) * 
00268             degrees_to_radians * this->angular_odom_scale;
00269         float yaw_rate = ss.yaw_rate * degrees_to_radians;
00270         
00271         // Integrate the displacements over time
00272         // If not the first odometry calculate the delta in displacements
00273         float vel_x = 0.0;
00274         float vel_y = 0.0;
00275         if(!this->first_odometry) {
00276             float delta_forward_displacement = 
00277                 forward_displacement - this->last_forward_displacement;
00278             double delta_time = (current_time-this->last_time).toSec();
00279             // Update accumulated odometries and calculate the x and y components 
00280             // of velocity
00281             this->odometry_w = yaw_displacement;
00282             float delta_odometry_x = 
00283                 delta_forward_displacement * std::cos(this->odometry_w);
00284             vel_x = delta_odometry_x / delta_time;
00285             this->odometry_x += delta_odometry_x;
00286             float delta_odometry_y = 
00287                 delta_forward_displacement * std::sin(this->odometry_w);
00288             vel_y = delta_odometry_y / delta_time;
00289             this->odometry_y += delta_odometry_y;
00290         } else {
00291             this->first_odometry = false;
00292         }
00293         // No matter what update the previouse (last) displacements
00294         this->last_forward_displacement = forward_displacement;
00295         this->last_yaw_displacement = yaw_displacement;
00296         this->last_time = current_time;
00297         
00298         // Create a Quaternion from the yaw displacement
00299         geometry_msgs::Quaternion quat = 
00300             tf::createQuaternionMsgFromYaw(yaw_displacement);
00301         
00302         // Publish the Transform odom->base_link
00303         if (this->broadcast_tf) {
00304             this->odom_trans.header.stamp = current_time;
00305             
00306             this->odom_trans.transform.translation.x = this->odometry_x;
00307             this->odom_trans.transform.translation.y = this->odometry_y;
00308             this->odom_trans.transform.translation.z = 0.0;
00309             this->odom_trans.transform.rotation = quat;
00310             
00311             //send the transform
00312             this->odom_broadcaster.sendTransform(this->odom_trans);
00313         }
00314         
00315         // Publish Odometry
00316         this->odom_msg.header.stamp = current_time;
00317         this->odom_msg.pose.pose.position.x = this->odometry_x;
00318         this->odom_msg.pose.pose.position.y = this->odometry_y;
00319         this->odom_msg.pose.pose.position.z = 0.0;
00320         this->odom_msg.pose.pose.orientation = quat;
00321         this->odom_msg.pose.covariance[0] = 0.00001;
00322         this->odom_msg.pose.covariance[7] = 0.00001;
00323         this->odom_msg.pose.covariance[14] = 1000000000000.0;
00324         this->odom_msg.pose.covariance[21] = 1000000000000.0;
00325         this->odom_msg.pose.covariance[28] = 1000000000000.0;
00326         this->odom_msg.pose.covariance[35] = 0.001;
00327         
00328         this->odom_msg.twist.twist.linear.x = vel_x;
00329         this->odom_msg.twist.twist.linear.y = vel_y;
00330         this->odom_msg.twist.twist.angular.z = yaw_rate;
00331         
00332         this->odom_pub.publish(this->odom_msg);
00333     }
00334     
00340     void motor_timeoutCallback(const ros::TimerEvent& e) {
00341         boost::mutex::scoped_lock lock(m_mutex);
00342         //ROS_INFO("Motor command timeout!  Setting target linear and angular velocities to be zero.");
00343         this->target_linear_vel = 0.0;
00344         this->target_angular_vel = 0.0;
00345     }
00346     
00350     void cmd_velCallback(const geometry_msgs::Twist::ConstPtr& msg) {
00351         if (!this->connected)
00352             return;
00353         boost::mutex::scoped_lock lock(m_mutex);
00354         double x = msg->linear.x, z = msg->angular.z;
00355         if (this->invert_x) {
00356             x *= -1;
00357         }
00358         if (this->invert_z) {
00359             z *= -1;
00360         }
00361         if (this->max_linear_vel != 0.0) {
00362           if (abs(x) > this->max_linear_vel) {
00363             x = (x > 0) ? this->max_linear_vel : -this->max_linear_vel;
00364           }
00365         }
00366         if (this->max_angular_vel != 0.0) {
00367           if (abs(z) > this->max_angular_vel) {
00368             z = (z > 0) ? this->max_angular_vel : -this->max_angular_vel;
00369           }
00370         }
00371         this->target_linear_vel = x;
00372         this->target_angular_vel = z * radians_to_degrees; // Convert to degrees
00373 
00374         //ROS_INFO("Received motor command linear vel = %f, angular vel = %f.",
00375         //    this->target_linear_vel, this->target_angular_vel);
00376 
00377         this->motor_timeout_timer = 
00378             this->n->createTimer(
00379                 ros::Duration(this->segway_motor_timeout),
00380                 &SegwayRMPNode::motor_timeoutCallback, 
00381                 this, 
00382                 true);
00383     }
00384 private:
00385     // Functions
00386     void setupROSComms() {
00387         // Subscribe to command velocities
00388         this->cmd_velSubscriber = n->subscribe("cmd_vel", 1000, &SegwayRMPNode::cmd_velCallback, this);
00389         // Advertise the SegwayStatusStamped
00390         this->segway_status_pub = n->advertise<segway_rmp::SegwayStatusStamped>("segway_status", 1000);
00391         // Advertise the Odometry Msg
00392         this->odom_pub = n->advertise<nav_msgs::Odometry>("odom", 50);
00393     }
00394     
00395     void setupSegwayRMP() {
00396         std::stringstream ss;
00397         ss << "Connecting to Segway RMP via ";
00398         this->segway_rmp = new segwayrmp::SegwayRMP(this->interface_type, this->segway_rmp_type);
00399         if (this->interface_type_str == "serial") {
00400             ss << "serial on serial port: " << this->serial_port;
00401             this->segway_rmp->configureSerial(this->serial_port);
00402         } else if (this->interface_type_str == "usb") {
00403             ss << "usb ";
00404             if (this->usb_selector == "serial_number") {
00405                 ss << "identified by the device serial number: " << this->serial_number;
00406                 this->segway_rmp->configureUSBBySerial(this->serial_number);
00407             }
00408             if (this->usb_selector == "description") {
00409                 ss << "identified by the device description: " << this->usb_description;
00410                 this->segway_rmp->configureUSBByDescription(this->usb_description);
00411             }
00412             if (this->usb_selector == "index") {
00413                 ss << "identified by the device index: " << this->usb_index;
00414                 this->segway_rmp->configureUSBByIndex(this->usb_index);
00415             }
00416         }
00417         ROS_INFO("%s", ss.str().c_str());
00418         
00419         // Set the instance variable
00420         segwayrmp_node_instance = this;
00421         
00422         // Set callbacks for segway data and messages
00423         this->segway_rmp->setStatusCallback(handleStatusWrapper);
00424         this->segway_rmp->setLogMsgCallback("debug", handleDebugMessages);
00425         this->segway_rmp->setLogMsgCallback("info", handleInfoMessages);
00426         this->segway_rmp->setLogMsgCallback("error", handleErrorMessages);
00427     }
00428     
00429     int getParameters() {
00430         // Get Interface Type
00431         n->param("interface_type", this->interface_type_str, std::string("serial"));
00432         // Get Configurations based on Interface Type
00433         if (this->interface_type_str == "serial") {
00434             this->interface_type = segwayrmp::serial;
00435             n->param("serial_port", this->serial_port, std::string("/dev/ttyUSB0"));
00436         } else if (this->interface_type_str == "usb") {
00437             this->interface_type = segwayrmp::usb;
00438             n->param("usb_selector", this->usb_selector, std::string("index"));
00439             if (this->usb_selector == "index") {
00440                 n->param("usb_index", this->usb_index, 0);
00441             } else if (this->usb_selector == "serial_number") {
00442                 n->param("usb_serial_number", this->serial_number, std::string("00000000"));
00443                 if (this->serial_number == std::string("00000000")) {
00444                     ROS_WARN("The serial_number parameter is set to the default 00000000, which shouldn't work.");
00445                 }
00446             } else if (this->usb_selector == "description") {
00447                 n->param("usb_description", this->serial_number, std::string("Robotic Mobile Platform"));
00448             } else {
00449                 ROS_ERROR(
00450                     "Invalid USB selector: %s, valid types are 'index', 'serial_number', and 'description'.", 
00451                     this->usb_selector.c_str());
00452                 return 1;
00453             }
00454         } else {
00455             ROS_ERROR(
00456                 "Invalid interface type: %s, valid interface types are 'serial' and 'usb'.",
00457                 this->interface_type_str.c_str());
00458             return 1;
00459         }
00460         // Get Setup Motor Timeout
00461         n->param("motor_timeout", this->segway_motor_timeout, 0.5);
00462         // Get frame id parameter
00463         n->param("frame_id", frame_id, std::string("base_link"));
00464         this->sss_msg.header.frame_id = this->frame_id;
00465         this->odom_trans.header.frame_id = "odom";
00466         this->odom_trans.child_frame_id = this->frame_id;
00467         this->odom_msg.header.frame_id = "odom";
00468         this->odom_msg.child_frame_id = this->frame_id;
00469         // Get cmd_vel inversion parameters
00470         n->param("invert_linear_vel_cmds", invert_x, false);
00471         n->param("invert_angular_vel_cmds", invert_z, false);
00472         // Get option for enable/disable tf broadcasting
00473         n->param("broadcast_tf", this->broadcast_tf, true);
00474         // Get the segway rmp type
00475         std::string segway_rmp_type_str;
00476         n->param("rmp_type", segway_rmp_type_str, std::string("200/400"));
00477         if (segway_rmp_type_str == "200/400") {
00478             this->segway_rmp_type = segwayrmp::rmp200;
00479         } else if (segway_rmp_type_str == "50/100") {
00480             this->segway_rmp_type = segwayrmp::rmp100;
00481         } else {
00482             ROS_ERROR(
00483                 "Invalid rmp type: %s, valid rmp types are '200/400' and '50/100'.",
00484                 segway_rmp_type_str.c_str());
00485             return 1;
00486         }
00487 
00488         // Get the linear acceleration limits in m/s^2.  Zero means infinite.
00489         n->param("linear_pos_accel_limit", this->linear_pos_accel_limit, 0.0);
00490         n->param("linear_neg_accel_limit", this->linear_neg_accel_limit, 0.0);
00491 
00492         // Get the angular acceleration limits in deg/s^2.  Zero means infinite.
00493         n->param("angular_pos_accel_limit", this->angular_pos_accel_limit, 0.0);
00494         n->param("angular_neg_accel_limit", this->angular_neg_accel_limit, 0.0);
00495         
00496         // Check for valid acceleration limits
00497         if (this->linear_pos_accel_limit < 0) {
00498             ROS_ERROR("Invalid linear positive acceleration limit of %f (must be non-negative).",
00499                 this->linear_pos_accel_limit);
00500             return 1;
00501         }
00502         if (this->linear_neg_accel_limit < 0) {
00503             ROS_ERROR("Invalid linear negative acceleration limit of %f (must be non-negative).",
00504                 this->linear_neg_accel_limit);
00505             return 1;
00506         }
00507         if (this->angular_pos_accel_limit < 0) {
00508             ROS_ERROR("Invalid angular positive acceleration limit of %f (must be non-negative).",
00509                 this->angular_pos_accel_limit);
00510             return 1;
00511         }
00512         if (this->angular_neg_accel_limit < 0) {
00513             ROS_ERROR("Invalid angular negative acceleration limit of %f (must be non-negative).",
00514                 this->angular_neg_accel_limit);
00515             return 1;
00516         }
00517 
00518         ROS_INFO("Accel limits: linear: pos = %f, neg = %f, angular: pos = %f, neg = %f.",
00519             this->linear_pos_accel_limit, this->linear_neg_accel_limit, 
00520             this->angular_pos_accel_limit, this->angular_neg_accel_limit);
00521 
00522         // Get velocity limits. Zero means no limit
00523         n->param("max_linear_vel", this->max_linear_vel, 0.0);
00524         n->param("max_angular_vel", this->max_angular_vel, 0.0);
00525         
00526         if (this->max_linear_vel < 0) {
00527             ROS_ERROR("Invalid max linear velocity limit of %f (must be non-negative).",
00528                 this->max_linear_vel);
00529             return 1;
00530         }
00531  
00532         if (this->max_angular_vel < 0) {
00533             ROS_ERROR("Invalid max angular velocity limit of %f (must be non-negative).",
00534                 this->max_angular_vel);
00535             return 1;
00536         }
00537 
00538         ROS_INFO("Velocity limits: linear: %f, angular: %f.",
00539             this->max_linear_vel, this->max_angular_vel); 
00540         
00541         // Convert the linear acceleration limits to have units of (m/s^2)/20 since
00542         // the movement commands are sent to the Segway at 20Hz.
00543         this->linear_pos_accel_limit /= 20;
00544         this->linear_neg_accel_limit /= 20;
00545 
00546         // Convert the angular acceleration limits to have units of (deg/s^2)/20 since
00547         // the movement commands are sent to the Segway at 20Hz.
00548         this->angular_pos_accel_limit /= 20;
00549         this->angular_neg_accel_limit /= 20;
00550 
00551         // Get the scale correction parameters for odometry
00552         n->param("linear_odom_scale", this->linear_odom_scale, 1.0);
00553         n->param("angular_odom_scale", this->angular_odom_scale, 1.0);
00554 
00555         // Check if a software odometry reset is required
00556         n->param("reset_odometry", this->reset_odometry, false);
00557         n->param("odometry_reset_duration", this->odometry_reset_duration, 1.0);
00558     
00559         return 0;
00560     }
00561     
00562     // Variables
00563     ros::NodeHandle * n;
00564     
00565     ros::Timer keep_alive_timer;
00566     
00567     ros::Subscriber cmd_velSubscriber;
00568     ros::Publisher segway_status_pub;
00569     ros::Publisher odom_pub;
00570     tf::TransformBroadcaster odom_broadcaster;
00571     
00572     segwayrmp::SegwayRMP * segway_rmp;
00573     
00574     std::string interface_type_str;
00575     segwayrmp::InterfaceType interface_type;
00576     segwayrmp::SegwayRMPType segway_rmp_type;
00577     std::string serial_port;
00578     std::string usb_selector;
00579     std::string serial_number;
00580     std::string usb_description;
00581     int usb_index;
00582     
00583     double segway_motor_timeout;
00584     ros::Timer motor_timeout_timer;
00585     
00586     std::string frame_id;
00587     bool invert_x, invert_z;
00588     bool broadcast_tf;
00589     
00590     double linear_vel;
00591     double angular_vel; // The angular velocity in deg/s
00592 
00593     double target_linear_vel;  // The ideal linear velocity in m/s
00594     double target_angular_vel; // The ideal angular velocity in deg/s
00595 
00596     double linear_pos_accel_limit;  // The max linear acceleration in (m/s^2)/20
00597     double linear_neg_accel_limit;  // The max linear deceleration in (m/s^2)/20
00598     double angular_pos_accel_limit; // The max angular acceleration in (deg/s^2)/20
00599     double angular_neg_accel_limit; // The max angular deceleration in (deg/s^2)/20
00600 
00601     double linear_odom_scale;       // linear odometry scale correction 
00602     double angular_odom_scale;      // angular odometry scale correction
00603 
00604     double max_linear_vel;  // maximum allowed magnitude of velocity
00605     double max_angular_vel;
00606     
00607     bool connected;
00608     
00609     segway_rmp::SegwayStatusStamped sss_msg;
00610     geometry_msgs::TransformStamped odom_trans;
00611     nav_msgs::Odometry odom_msg;
00612     
00613     int count;
00614     
00615     bool first_odometry;
00616     float last_forward_displacement;
00617     float last_yaw_displacement;
00618     float odometry_x;
00619     float odometry_y;
00620     float odometry_w;
00621     ros::Time last_time;
00622     
00623     boost::mutex m_mutex;
00624 
00625     // Hardware reset of integrators can sometimes fail.
00626     // These help in performing a software reset.
00627     bool reset_odometry;
00628     double odometry_reset_duration;
00629     ros::Time odometry_reset_start_time;
00630     double initial_integrated_forward_position;
00631     double initial_integrated_left_wheel_position;
00632     double initial_integrated_right_wheel_position;
00633     double initial_integrated_turn_position;
00634     
00635 };
00636 
00637 // Callback wrapper
00638 void handleStatusWrapper(segwayrmp::SegwayStatus::Ptr &ss) {
00639     segwayrmp_node_instance->handleStatus(ss);
00640 }
00641 
00642 int main(int argc, char **argv) {
00643     ros::init(argc, argv, "segway_rmp_node");
00644     
00645     SegwayRMPNode segwayrmp_node;
00646     
00647     segwayrmp_node.run();
00648     
00649     return 0;
00650 }


segway_rmp
Author(s): William Woodall
autogenerated on Mon Oct 6 2014 07:30:54