Ros.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, CoroWare
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Stanford U. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "Ros.h"
00031 #include "corobot.h"
00032 #include <QImage>
00033 #include <QGraphicsTextItem>
00034 #include <QPainter>
00035 #include <QGraphicsView>
00036 #include <math.h>
00037 #include <QImageReader>
00038 
00039 #include <vector>
00040 
00041 using namespace std;
00042 
00043 Ros::Ros(){
00044 
00045         speed_value = 100;
00046         speed_x = 0;
00047         speed_a = 0;
00048 
00049         arm_px = (double)UPPER_ARM_LENGTH_INCHES * (double)INCHES_TO_METERS;
00050         arm_py = (double)LOWER_ARM_LENGTH_INCHES * (double)INCHES_TO_METERS;
00051 
00052         pan = 0;
00053         tilt = 0;
00054         move_speed_level = 0;
00055         turning_speed_level = 0;
00056 
00057         turningRight = false;
00058         turningLeft = false;
00059 
00060 
00061         hokuyo_points_ = new Hokuyo_Points [683]; //hokuyo data
00062         speed_left = 0;
00063         speed_right = 0;
00064 
00065         initialized = false;
00066 
00067         ir01 = 0;
00068         ir02 = 0;
00069         bumper_data[0] = 0;
00070         bumper_data[1] = 0;
00071         bumper_data[2] = 0;
00072         bumper_data[3] = 0;
00073 }
00074 
00075 
00076 
00077 Ros::~Ros(){
00078     delete [] hokuyo_points_;
00079     hokuyo_points_ = NULL;
00080     if(driveControl_pub)
00081         this->motor_stop();
00082 }
00083 
00084 void Ros::subscribe()
00085 //function that initialize every ros variables declared
00086 
00087 {
00088     ros::NodeHandle n;
00089     ros::NodeHandle nh("~");
00090     nh.param("cameraRear_jpeg_compression", cameraRear_jpeg_compression, false); // true if the jpeg version of the rear camera should be used
00091     nh.param("cameraFront_jpeg_compression", cameraFront_jpeg_compression, false); // true if the jpeg version of the front camera should be used
00092     nh.param("Lynxmotion_al5a", arm_al5a, false); // true if there is an al5a arm
00093     nh.param("PhantomX_Pincher", arm_pincher, false); // true if there is a pincher arm
00094     nh.param("PhantomX_Reactor", arm_reactor, false); // true if there is a reactor arm
00095     nh.param("corobot_arm_ssc32", arm_old_ssc32, false); // true if there is an old ssc32 arm
00096     nh.param("corobot_arm_phidget", arm_old_phidget, false); // true if there is an old phidget arm
00097 
00098     emit arm_model(arm_al5a, arm_pincher, arm_reactor, arm_old_ssc32 || arm_old_phidget);
00099 
00100     //Advertise topics
00101     driveControl_pub = n.advertise<corobot_msgs::MotorCommand>("PhidgetMotor", 100);
00102     velocityValue_pub = n.advertise<std_msgs::Int32>("velocityValue", 100);
00103     moveArm_pub = n.advertise<corobot_msgs::MoveArm>("armPosition", 100);
00104     pan_tilt_control = n.advertise<corobot_msgs::PanTilt>("pantilt",5);
00105 
00106     //Subscribe to topics
00107     imu=n.subscribe("imu_data",1000, &Ros::imuCallback,this);
00108     magnetic=n.subscribe("magnetic_data",1000, &Ros::magCallback,this);
00109     velocity= n.subscribe("odometry", 1000, &Ros::velocityCallback,this);
00110     ir= n.subscribe("infrared_data", 1000, &Ros::irCallback,this);
00111     power= n.subscribe<corobot_msgs::PowerMsg>("power_data", 1000, &Ros::powerCallback,this);
00112     bumper= n.subscribe("bumper_data", 1000, &Ros::bumperCallback,this);
00113     gps= n.subscribe("fix", 1000, &Ros::gpsCallback,this);
00114     scan= n.subscribe("scan", 1000, &Ros::scanCallback,this);
00115     kinect_rgb = n.subscribe("/camera/rgb/image_color/compressed",100,&Ros::kinectrgbCallback,this);
00116     kinect_depth = n.subscribe("/camera/depth/image",100,&Ros::kinectdepthCallback,this);
00117     takepic_sub = n.subscribe("takepicture", 10, &Ros::takepicCallback, this);
00118     map_image = n.subscribe("/map_image/full_with_position/compressed",10, &Ros::mapCallback,this);
00119 
00120     //Use jpeg stream of raw images for the camera(s)
00121     if(cameraRear_jpeg_compression)
00122         rear_cam = n.subscribe("REAR/image_raw/compressed",10, &Ros::rear_camCallback_compressed,this);
00123     else
00124         rear_cam = n.subscribe("REAR/image_raw",10, &Ros::rear_camCallback,this);
00125     if(cameraFront_jpeg_compression)
00126         ptz_cam = n.subscribe("PTZ/image_raw/compressed",10, &Ros::ptz_camCallback_compressed,this);
00127     else
00128         ptz_cam = n.subscribe("PTZ/image_raw",10, &Ros::ptz_camCallback,this);
00129 
00130     kinect_selected = true;
00131     pan = 0;
00132     tilt = 0;
00133     timer = nh.createTimer(ros::Duration(2), &Ros::timerCallback, this); // used to send again the motor command after 2 seconds, in order to stop the robot after a wireless deconnection
00134     initialized = true;
00135 }
00136 
00137 void Ros::init(int argc, char *argv[]){
00138     ros::init(argc, argv,"GUI");
00139 
00140     this->subscribe();
00141     this->start();
00142 }
00143 
00144 void Ros::run(){
00145     ros::spin();
00146 }
00147 
00148 //subscribe to only the camera topics we are interested in
00149 void Ros::currentCameraTabChanged(int index)
00150 {
00151     if (initialized == true)
00152     {
00153             ros::NodeHandle n;
00154             if(index == 0) // PTZ Camera
00155             {
00156                 if(cameraFront_jpeg_compression)
00157                     ptz_cam = n.subscribe("PTZ/image_raw/compressed",10, &Ros::ptz_camCallback_compressed,this);
00158                 else
00159                     ptz_cam = n.subscribe("PTZ/image_raw",10, &Ros::ptz_camCallback,this);
00160                 rear_cam.shutdown();
00161                 kinect_rgb.shutdown();
00162                 kinect_depth.shutdown();
00163             }
00164             else if(index == 1) // rear Camera
00165             {
00166                 if(cameraRear_jpeg_compression)
00167                     rear_cam = n.subscribe("REAR/image_raw/compressed",10, &Ros::rear_camCallback_compressed,this);
00168                 else
00169                     rear_cam = n.subscribe("REAR/image_raw",10, &Ros::rear_camCallback,this);
00170                 ptz_cam.shutdown();
00171                 kinect_rgb.shutdown();
00172                 kinect_depth.shutdown();
00173             }
00174             else if (index == 2)
00175             {
00176                 rear_cam.shutdown();
00177                 ptz_cam.shutdown();
00178                 kinect_rgb.shutdown();
00179                 kinect_depth.shutdown();
00180             }
00181             else if(index == 3) // Kinect RGB
00182             {
00183                 kinect_rgb = n.subscribe("/camera/rgb/image_color/compressed",100,&Ros::kinectrgbCallback,this);
00184                 rear_cam.shutdown();
00185                 ptz_cam.shutdown();
00186                 kinect_depth.shutdown();
00187             }
00188             else if(index == 4) // Kinect Depth
00189             {
00190                 kinect_depth = n.subscribe("/camera/depth/image",100,&Ros::kinectdepthCallback,this);
00191                 rear_cam.shutdown();
00192                 kinect_rgb.shutdown();
00193                 ptz_cam.shutdown();
00194             }
00195     }
00196 }
00197 
00198 // Initialize the ROS node with the ROS_MASTER_URI and ROS_IP given by the user in the interface
00199 void Ros::init(int argc, char *argv[],const std::string & master,const std::string & host){
00200                 std::map<std::string,std::string> remappings;
00201                 remappings["__master"] = master;
00202                 remappings["__ip"] = host;
00203                 ros::init(remappings,"GUI");
00204 
00205                 this->subscribe();
00206                 this->start();
00207 }
00208 
00209 // Called when the user selects the fast speed movement, which is the fastest possible.
00210 void Ros::setSpeedFast(bool toggled){
00211         if(toggled)
00212         {
00213                 speed_value = 100;
00214                 std_msgs::Int32 msg;
00215                 msg.data = speed_value;
00216                 velocityValue_pub.publish(msg);
00217         }
00218 }
00219 
00220 // Called when the user selects the medium speed movement
00221 void Ros::setSpeedModerate(bool toggled){
00222         if(toggled)
00223         {
00224                 speed_value = 75;
00225                 std_msgs::Int32 msg;
00226                 msg.data = speed_value;
00227                 velocityValue_pub.publish(msg);
00228         }
00229 }
00230 
00231 // Called when the user selects the slow speed movement
00232 void Ros::setSpeedSlow(bool toggled){
00233         if(toggled)
00234         {
00235                 speed_value = 50;
00236                 std_msgs::Int32 msg;
00237                 msg.data = speed_value;
00238                 velocityValue_pub.publish(msg);
00239         }
00240 }
00241 
00242 // Kinect depth image callback
00243 void Ros::kinectdepthCallback(const sensor_msgs::Image::ConstPtr& msg){
00244     if(kinect_selected){
00245         unsigned char * copy;
00246         copy = (unsigned char*)malloc(msg->width*msg->height*3);
00247         float* tmp;
00248 
00249         for(int j=0;j<(msg->width*msg->height*3);j+=3){
00250             tmp = (float*)(&msg->data[0]+(j/3)*4);
00251             copy[j] = (char)((float)64*(float)(*tmp));
00252             copy[j+1] = (char)((float)64*(float)(*tmp));
00253             copy[j+2] = (char)((float)64*(float)(*tmp));
00254         }
00255 
00256     QImage *i = new QImage(copy,msg->width,msg->height,3*msg->width,QImage::Format_RGB888);
00257     QImage icopy = i->copy(0,0,msg->width,msg->height);
00258     if(i!=NULL){
00259                 emit update_kinectDepthcam(icopy); //we can't allow a thread to use the scene for the zoom and another one to modify the image as it would crash, so we do everything in one thread
00260         }
00261     free(copy);
00262     delete i;
00263 
00264 }
00265 }
00266 
00267 // Kinect rgb image callback
00268 void Ros::kinectrgbCallback(const sensor_msgs::CompressedImage::ConstPtr& msg){
00269     if(kinect_selected){
00270         QImage *i = new QImage();
00271         i->loadFromData(&msg->data[0],msg->data.size(),"JPEG");
00272         QImage icopy = i->scaled(640,480);
00273         if(i!=NULL){
00274                 emit update_kinectRGBcam(icopy); //we can't allow a thread to use the scene for the zoom and another one to modify the image as it would crash, so we do everything in one thread
00275         }
00276 
00277         delete i;
00278     }
00279 }
00280 
00281 // Odometry message callback
00282 void Ros::velocityCallback(const nav_msgs::Odometry::ConstPtr& msg){
00283     double linear, angular;
00284     linear = sqrt(msg->twist.twist.linear.x *msg->twist.twist.linear.x + msg->twist.twist.linear.y * msg->twist.twist.linear.y);
00285     angular = msg->twist.twist.angular.z;
00286 
00287     emit velocity_info(linear,angular);
00288 }
00289 
00290 // Infrared sensors callback
00291 void Ros::irCallback(const corobot_msgs::SensorMsg::ConstPtr& msg){
00292 
00293     if(msg->type == msg->INFRARED_FRONT)
00294         ir01 = (double)msg->value;
00295     else if(msg->type == msg->INFRARED_REAR)
00296         ir02 = (double)msg->value;
00297 
00298     emit irData(ir01,ir02);
00299 }
00300 
00301 // Battery voltage sensor callback
00302 void Ros::powerCallback(const corobot_msgs::PowerMsgConstPtr& msg){
00303     int percent;
00304     if(msg->volts>= 12)
00305         percent = 100;
00306     else
00307        percent = ((float)(msg->volts - BATTERY_EMPTY)/(float)(12-BATTERY_EMPTY))*100;
00308     emit battery_percent(percent);
00309     emit battery_volts((double)msg->volts);
00310 }
00311 
00312 // bumper sensors callback
00313 void Ros::bumperCallback(const corobot_msgs::SensorMsg::ConstPtr& msg){
00314      if (msg->type == msg->BUMPER)
00315      {
00316         bumper_data[msg->index] = msg->value;
00317         emit bumper_update(bumper_data[0], bumper_data[1], bumper_data[2], bumper_data[3]);
00318      }
00319      
00320 }
00321 
00322 // non-jpeg Image from the rear camera
00323 void Ros::rear_camCallback(const sensor_msgs::Image::ConstPtr& msg){
00324     QImage *i = new QImage(&msg->data[0],msg->width,msg->height,msg->step,QImage::Format_RGB888);
00325     QImage icopy = i->copy(0,0,msg->width,msg->height);
00326 
00327     if(i!=NULL){
00328         emit update_rearcam(icopy); //we can't allow a thread to use the scene for the zoom and another one to modify the image as it would crash, so we do everything in one thread
00329         }
00330     delete i;
00331 }
00332 
00333 //jpeg Image from the rear camera
00334 void Ros::rear_camCallback_compressed(const sensor_msgs::CompressedImage::ConstPtr& msg){
00335     QImage *i = new QImage();
00336     i->loadFromData(&msg->data[0],msg->data.size(),"JPEG");
00337     QImage icopy = i->copy(0,0,640,480);
00338     if(i!=NULL){
00339         emit update_rearcam(icopy); //we can't allow a thread to use the scene for the zoom and another one to modify the image as it would crash, so we do everything in one thread
00340         }
00341     delete i;
00342 }
00343 
00344 //non-jpeg Image from the front camera
00345 void Ros::ptz_camCallback(const sensor_msgs::Image::ConstPtr& msg){
00346     QImage *i = new QImage(&msg->data[0],msg->width,msg->height,msg->step,QImage::Format_RGB888);
00347     QImage icopy = i->copy(0,0,msg->width,msg->height);
00348 
00349     if(i!=NULL){
00350         emit update_ptzcam(icopy); //we can't allow a thread to use the scene for the zoom and another one to modify the image as it would crash, so we do everything in one thread
00351     }
00352 
00353     delete i;
00354 }
00355 
00356 //jpeg Image from the front camera
00357 void Ros::ptz_camCallback_compressed(const sensor_msgs::CompressedImage::ConstPtr& msg){
00358     QImage *i = new QImage();
00359     i->loadFromData(&msg->data[0],msg->data.size(),"JPEG");
00360     QImage icopy = i->copy(0,0,640,480);
00361     if(i!=NULL){
00362         emit update_ptzcam(icopy); //we can't allow a thread to use the scene for the zoom and another one to modify the image as it would crash, so we do everything in one thread
00363         }
00364     delete i;
00365 }
00366 
00367 // Jpeg image of the slam map
00368 void Ros::mapCallback(const sensor_msgs::CompressedImage::ConstPtr& msg){
00369     QImage *i = new QImage();
00370     i->loadFromData(&msg->data[0],msg->data.size(),"JPEG");
00371     QImage icopy = i->copy(0,0,i->height(),i->width());
00372     if(i!=NULL){   
00373                 emit update_mapimage(icopy); //we can't allow a thread to use the scene for the zoom and another one to modify the image as it would crash, so we do everything in one thread
00374             }
00375     delete i;
00376 }
00377 
00378 // gps data callback
00379 void Ros::gpsCallback(const sensor_msgs::NavSatFix::ConstPtr& msg){
00380     double lat = msg->latitude;
00381     double lon = msg->longitude;
00382 
00383         emit gps_lat(lat);
00384         emit gps_lon(lon);
00385         emit gps_coord(lat,lon);
00386 }
00387 
00388 // Laser Range Finder data callback
00389 void Ros::scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg){
00390       int index = 0;
00391 
00392       for(int it = 44; it<726; it++)
00393       {
00394 
00395           hokuyo_points_[it-44].distance = msg->ranges[it];
00396           hokuyo_points_[it-44].angle_step = msg->angle_increment;
00397           hokuyo_points_[it-44].index = it - 44;
00398 
00399           double radian = (index - 384 + 44) * msg->angle_increment;
00400 
00401           hokuyo_points_[it-44].x = -(hokuyo_points_[it-44].distance * sin (radian)); 
00402           hokuyo_points_[it-44].y = hokuyo_points_[it-44].distance * cos (radian); 
00403 
00404           index ++;
00405 
00406       }
00407 
00408 
00409       emit hokuyo_update(hokuyo_points_);
00410 
00411 }
00412 
00413 // Command from the joystick to save an image from the camera
00414 void Ros::takepicCallback(const corobot_msgs::takepic::ConstPtr &msg)
00415 {
00416     if(msg->take)
00417     {
00418         emit save_image(true);
00419     }
00420 }
00421 
00422 // Imu callback
00423 void Ros::imuCallback(const sensor_msgs::Imu::ConstPtr &msg)
00424 {
00425 
00426     double acc_x, acc_y, acc_z, ang_x, ang_y, ang_z;
00427 
00428     acc_x = (double)msg->linear_acceleration.x;
00429     acc_y = (double)msg->linear_acceleration.y;
00430     acc_z = (double)msg->linear_acceleration.z;
00431     ang_x = (double)msg->angular_velocity.x;
00432     ang_y = (double)msg->angular_velocity.y;
00433     ang_z = (double)msg->angular_velocity.z;
00434 
00435 
00436     emit imu_data(acc_x,acc_y,acc_z,ang_x,ang_y,ang_z);
00437 }
00438 
00439 void Ros::magCallback(const sensor_msgs::MagneticField::ConstPtr &msg)
00440 {
00441     double mag_x, mag_y, mag_z;
00442 
00443     mag_x = (double)msg->magnetic_field.x;
00444     mag_y = (double)msg->magnetic_field.y;
00445     mag_z = (double)msg->magnetic_field.z;
00446 
00447     emit magnetic_data(mag_x,mag_y,mag_z);
00448 }
00449 
00450 // Open of close the gripper. If state is true open the gripper, false close the gripper
00451 void Ros::moveGripper(bool state){
00452     if(state)
00453         Ros::openGripper();
00454     else
00455         Ros::closeGripper();
00456 }
00457 
00458 // turn the wrist
00459 void Ros::turnWrist(float angle){
00460 
00461         if(moveArm_pub)
00462         {
00463             corobot_msgs::MoveArm msg;
00464 
00465             /* Do some angle conversions. First we add PI/2 because the default position is actually at angle Pi/2. 
00466                Then we make sure that the angle is not too high or too low
00467                 Finally we convert to a value in degrees
00468             */
00469             double temp = -angle + M_PI/2;
00470             if  (-angle > M_PI)
00471                 temp =  - 2*M_PI - angle + M_PI/2;
00472             if (temp < 0)
00473                 temp = 2*M_PI + temp;
00474             if(0 < temp || temp > M_PI)
00475                 temp = (double)((int)(temp * 180/M_PI) % 180); // execute temp % M_PI, the conversion to int in degree is necessary as the modulus works with integers.
00476 
00477 
00478                 //temporarily move both the flex and rotation see the GUI doesn't do the difference
00479             msg.index = msg.WRIST_FLEX;
00480             msg.position = temp;
00481 
00482             moveArm_pub.publish(msg);
00483 
00484             msg.index = msg.WRIST_ROTATION;
00485             msg.position = temp;
00486 
00487             moveArm_pub.publish(msg);
00488         }
00489 }
00490 
00491 // open the gripper
00492 void Ros::openGripper(){
00493 
00494         if(moveArm_pub)
00495         {
00496                 corobot_msgs::MoveArm msg;
00497 
00498                 msg.index = msg.GRIPPER;
00499                 msg.position = 0;
00500 
00501                 moveArm_pub.publish(msg);
00502         }
00503 }
00504 
00505 // close the gripper
00506 void Ros::closeGripper(){
00507 
00508         if(moveArm_pub)
00509         {
00510                 corobot_msgs::MoveArm msg;
00511 
00512                 msg.index = msg.GRIPPER;
00513                 msg.position = 180;
00514 
00515                 moveArm_pub.publish(msg);
00516         }
00517 }
00518 
00519 // timer callback, send the motor command again to avoid the motors from stopping. This method is used to stop the robot when wireless problem happen
00520 void Ros::timerCallback(const ros::TimerEvent& event)
00521 {
00522         corobot_msgs::MotorCommand msg;
00523 
00524         msg.leftSpeed = speed_left;
00525         msg.rightSpeed = speed_right;
00526         msg.secondsDuration = 3;
00527         msg.acceleration = 50;
00528 
00529         driveControl_pub.publish(msg);
00530  
00531 }
00532 
00533 //Decrease speed when no robot movement buttons or key is pressed
00534 bool Ros::decrease_speed()
00535 {
00536     if(driveControl_pub)
00537     {
00538 
00539         corobot_msgs::MotorCommand msg;
00540 
00541         if(turningLeft)
00542         {
00543             speed_left = -speed_value;
00544             speed_right = speed_value;
00545         }
00546         else if(turningRight)
00547         {
00548             speed_left = speed_value;
00549             speed_right = -speed_value;
00550         }
00551         else
00552         {
00553             speed_left = 0;
00554             speed_right = 0;
00555         }
00556 
00557         msg.leftSpeed = speed_left;
00558         msg.rightSpeed = speed_right;
00559         msg.secondsDuration = 3;
00560         msg.acceleration = 50;
00561 
00562         driveControl_pub.publish(msg);
00563 
00564     }
00565     return true;
00566 }
00567 
00568 //Increase speed when the move forward button or key is pressed
00569 bool Ros::increase_speed()
00570 {
00571 
00572     corobot_msgs::MotorCommand msg;
00573         if(driveControl_pub)
00574         {
00575                         speed_left += speed_value;
00576                         speed_right += speed_value;
00577 
00578                         if(speed_left >speed_value)
00579                                 speed_left = speed_value;
00580                         if(speed_right >speed_value)
00581                                 speed_right = speed_value;
00582                         msg.leftSpeed = speed_left;
00583                         msg.rightSpeed = speed_right;
00584                         msg.secondsDuration = 3;
00585                         msg.acceleration = 50;
00586 
00587 
00588                     driveControl_pub.publish(msg);
00589                 return true;
00590         }
00591         else
00592                 return false;
00593 }
00594 
00595 //Increase speed when the move backward button or key is pressed
00596 bool Ros::increase_backward_speed()
00597 {
00598     corobot_msgs::MotorCommand msg;
00599         if(driveControl_pub)
00600         {
00601                 speed_left -= speed_value;
00602                 speed_right -= speed_value;
00603 
00604                 if(speed_left <-speed_value)
00605                         speed_left = -speed_value;
00606                 if(speed_right <-speed_value)
00607                         speed_right = -speed_value;
00608 
00609                 msg.leftSpeed = speed_left;
00610                 msg.rightSpeed = speed_right;
00611                 msg.secondsDuration = 3;
00612                 msg.acceleration = 50;
00613 
00614                 driveControl_pub.publish(msg);
00615                 return true;
00616         }
00617         else
00618                 return false;
00619 }
00620 
00621 //Turn the robot left
00622 bool Ros::turn_left()
00623 {
00624     corobot_msgs::MotorCommand msg;
00625 
00626         if(driveControl_pub)
00627         {
00628                 if((speed_left+speed_right)<0)
00629                 {
00630                     speed_left += speed_value;
00631                     speed_right -= speed_value;
00632                 }
00633                 else
00634                 {
00635                     speed_left -= speed_value;
00636                     speed_right += speed_value;
00637                 }
00638 
00639                 if(speed_left >speed_value)
00640                     speed_left = speed_value;
00641                 if(speed_right >speed_value)
00642                     speed_right = speed_value;
00643                 if(speed_left <-speed_value)
00644                     speed_left = -speed_value;
00645                 if(speed_right <-speed_value)
00646                     speed_right = -speed_value;
00647 
00648 
00649                 msg.leftSpeed = speed_left;
00650                 msg.rightSpeed = speed_right;
00651                 msg.secondsDuration = 3;
00652                 msg.acceleration = 50;
00653 
00654                 driveControl_pub.publish(msg);
00655                 turningLeft = true;
00656                 turningRight = false;
00657                 return true;
00658         }
00659         else
00660                 return false;
00661 }
00662 
00663 // Turn the robot right
00664 bool Ros::turn_right()
00665 {
00666     corobot_msgs::MotorCommand msg;
00667 
00668         if(driveControl_pub)
00669         {
00670                 if((speed_left+speed_right)<0)
00671                 {
00672                     speed_left -= speed_value;
00673                     speed_right += speed_value;
00674                 }
00675                 else
00676                 {
00677                     speed_left += speed_value;
00678                     speed_right -= speed_value;
00679                 }
00680                 if(speed_left >speed_value)
00681                     speed_left = speed_value;
00682                 if(speed_right >speed_value)
00683                     speed_right = speed_value;
00684                 if(speed_left <-speed_value)
00685                     speed_left = -speed_value;
00686                 if(speed_right <-speed_value)
00687                     speed_right = -speed_value;
00688 
00689                 msg.leftSpeed = speed_left;
00690                 msg.rightSpeed = speed_right;
00691                 msg.secondsDuration = 3;
00692                 msg.acceleration = 50;
00693 
00694                 driveControl_pub.publish(msg);
00695                 turningRight = true;
00696                 turningLeft = false;
00697                 return true;
00698         }
00699         else
00700                 return false;
00701 }
00702 
00703 // stop the robot from turning
00704 bool Ros::stop_turn()
00705 {
00706     corobot_msgs::MotorCommand msg;
00707 
00708         if(driveControl_pub)
00709         {       
00710                 if((speed_left+speed_right)<0)
00711                 {
00712                         speed_left = -speed_value;
00713                         speed_right = -speed_value;
00714                 }
00715                 else if((speed_left+speed_right)>0)
00716                 {
00717                         speed_left = +speed_value;
00718                         speed_right = +speed_value;
00719                 }
00720                 else
00721                 {
00722                         speed_left = 0;
00723                         speed_right = 0;
00724                 }
00725         
00726                 msg.leftSpeed = speed_left;
00727                 msg.rightSpeed = speed_right;
00728                 msg.secondsDuration = 3;
00729                 msg.acceleration = 50;
00730 
00731                 driveControl_pub.publish(msg);
00732                 turningLeft = false;
00733                 turningRight = false;
00734                 return true;
00735         }
00736         else
00737                 return false;
00738 }
00739 
00740 // stop the motors
00741 bool Ros::motor_stop()
00742 {
00743     corobot_msgs::MotorCommand msg;
00744 
00745         if(driveControl_pub)
00746         {
00747                 msg.leftSpeed = 0;
00748                 msg.rightSpeed = 0;
00749                 msg.secondsDuration = 0;
00750                 msg.acceleration = 50;
00751                 driveControl_pub.publish(msg);
00752                 return true;
00753         }
00754         else
00755                 return false;
00756 }
00757 
00758 // move the shoulder of the arm
00759 void Ros::moveShoulderArm(double shoulder)
00760 {
00761         if(moveArm_pub)
00762         {
00763                 corobot_msgs::MoveArm msg;
00764 
00765                 msg.index = msg.SHOULDER;
00766                 msg.position = shoulder *180/M_PI; //convertion from radian to degree
00767 
00768                 moveArm_pub.publish(msg);
00769         }
00770 }
00771 
00772 // move the elbow arm
00773 void Ros::moveElbowArm(double elbow)
00774 {
00775         if(moveArm_pub)
00776         {
00777                 corobot_msgs::MoveArm msg;
00778 
00779                 msg.index = msg.ELBOW;
00780                 msg.position = elbow *180/M_PI; //convertion from radian to degree
00781 
00782                 moveArm_pub.publish(msg);
00783         }
00784 }
00785 
00786 // rotate the arm
00787 void Ros::rotateArm(double angle)
00788 {
00789         if(moveArm_pub)
00790         {
00791                 corobot_msgs::MoveArm msg;
00792 
00793                 msg.index = msg.BASE_ROTATION;
00794                 msg.position = angle *181/M_PI; //convertion from radian to degree
00795 
00796                 moveArm_pub.publish(msg);
00797         }
00798 }
00799 
00800 // reset the arm position
00801 void Ros::ResetArm()
00802 {
00803         if(moveArm_pub)
00804         {
00805                 corobot_msgs::MoveArm msg;
00806 
00807                 msg.index = msg.SHOULDER;
00808                 msg.position = 0;
00809                 moveArm_pub.publish(msg);
00810 
00811                 msg.index = msg.ELBOW;
00812                 msg.position = 0; 
00813                 moveArm_pub.publish(msg);
00814 
00815                 msg.index = msg.WRIST_FLEX;
00816                 msg.position = 90; 
00817                 moveArm_pub.publish(msg);
00818 
00819                 msg.index = msg.WRIST_ROTATION;
00820                 msg.position = 90; 
00821                 moveArm_pub.publish(msg);
00822 
00823                 msg.index = msg.GRIPPER;
00824                 msg.position = 0; 
00825                 moveArm_pub.publish(msg);
00826 
00827                 msg.index = msg.BASE_ROTATION;
00828                 msg.position = 90; 
00829                 moveArm_pub.publish(msg);
00830         }
00831 }
00832 
00833 void Ros::Pan_control(int value) // get the user interaction to control the pan camera and send it to the ros node
00834 {
00835     pan = value;
00836     corobot_msgs::PanTilt msg;
00837     msg.pan = pan;
00838     msg.tilt = tilt;
00839     msg.reset = 0;
00840     pan_tilt_control.publish(msg);
00841 }
00842 
00843 void Ros::Tilt_control(int value) // get the user interaction to control the tilt camera and send it to the ros node
00844 {
00845     tilt = value;
00846     corobot_msgs::PanTilt msg;
00847     msg.pan = pan;
00848     msg.tilt = tilt;
00849     msg.reset = 0;
00850     pan_tilt_control.publish(msg);
00851 }
00852 


corobot_teleop
Author(s):
autogenerated on Sun Oct 5 2014 23:18:16