vpROSRobot.cpp
Go to the documentation of this file.
00001 /****************************************************************************
00002  *
00003  * $Id: vpROSRobot.cpp 3530 2012-01-03 10:52:12Z fpasteau $
00004  *
00005  * This file is part of the ViSP software.
00006  * Copyright (C) 2005 - 2012 by INRIA. All rights reserved.
00007  *
00008  * This software is free software; you can redistribute it and/or
00009  * modify it under the terms of the GNU General Public License
00010  * ("GPL") version 2 as published by the Free Software Foundation.
00011  * See the file LICENSE.txt at the root directory of this source
00012  * distribution for additional information about the GNU GPL.
00013  *
00014  * For using ViSP with software that can not be combined with the GNU
00015  * GPL, please contact INRIA about acquiring a ViSP Professional
00016  * Edition License.
00017  *
00018  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
00019  *
00020  * This software was developed at:
00021  * INRIA Rennes - Bretagne Atlantique
00022  * Campus Universitaire de Beaulieu
00023  * 35042 Rennes Cedex
00024  * France
00025  * http://www.irisa.fr/lagadic
00026  *
00027  * If you have questions regarding the use of this file, please contact
00028  * INRIA at visp@inria.fr
00029  *
00030  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
00031  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
00032  *
00033  *
00034  * Description:
00035  * vpRobot implementation for ROS middleware
00036  *
00037  * Authors:
00038  * Francois Pasteau
00039  *
00040  *****************************************************************************/
00041 
00042 
00048 #include <visp/vpHomogeneousMatrix.h>
00049 #include <visp/vpRobotException.h>
00050 #include <visp_ros/vpROSRobot.h>
00051 #include <visp/vpDebug.h>
00052 #include <iostream>
00053 #include <ros/ros.h>
00054 #include <ros/time.h>
00055 #include <sstream>
00056 
00058 vpROSRobot::vpROSRobot():
00059   isInitialized(false),
00060   odom_mutex(true),
00061   q(0,0,0,1),
00062   p(0,0,0),
00063   _sec(0),
00064   _nsec(0),
00065   displacement(6),
00066   pose_prev(6),
00067   _master_uri("http://127.0.0.1:11311"),
00068   _topic_cmd("/RosAria/cmd_vel"),
00069   _topic_odom("odom"),
00070   _nodespace("")
00071 {
00072 
00073 }
00074 
00075 
00076 
00078 vpROSRobot::~vpROSRobot()
00079 {
00080   if(isInitialized){
00081     isInitialized = false;
00082     spinner->stop();
00083     delete spinner;
00084     delete n;
00085   }
00086 }
00087 
00093 void vpROSRobot::init(int argc, char **argv)
00094 {
00095   std::cout << "ici 1\n";
00096   if(!isInitialized){
00097     if(!ros::isInitialized()) ros::init(argc, argv, "visp_node", ros::init_options::AnonymousName);
00098     n = new ros::NodeHandle;
00099     cmdvel = n->advertise<geometry_msgs::Twist>(_nodespace + _topic_cmd, 1);
00100     odom = n->subscribe(_nodespace + _topic_odom, 1, &vpROSRobot::odomCallback,this,ros::TransportHints().tcpNoDelay());
00101     spinner = new ros::AsyncSpinner(1);
00102     spinner->start();
00103     isInitialized = true;
00104     std::cout << "ici 2\n";
00105   }
00106 }
00107 
00112 void vpROSRobot::init(){
00113   if(ros::isInitialized() && ros::master::getURI() != _master_uri){
00114     throw (vpRobotException(vpRobotException::constructionError,
00115                             "ROS robot already initialised with a different master_URI (" + ros::master::getURI() +" != " + _master_uri + ")") );
00116   }
00117   if(!isInitialized){
00118     int argc = 2;
00119     char *argv[2];
00120     argv[0] = new char [255];
00121     argv[1] = new char [255];
00122 
00123     std::string exe = "ros.exe", arg1 = "__master:=";
00124     strcpy(argv[0], exe.c_str());
00125     arg1.append(_master_uri);
00126     strcpy(argv[1], arg1.c_str());
00127     init(argc, argv);
00128     delete [] argv[0];
00129     delete [] argv[1];
00130   }
00131 }
00132 
00133 
00144 void vpROSRobot::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
00145 {
00146   geometry_msgs::Twist msg;
00147   if (frame == vpRobot::REFERENCE_FRAME)
00148   {
00149     msg.linear.x = vel[0];
00150     msg.linear.y = vel[1];
00151     msg.linear.z = vel[2];
00152     msg.angular.x = vel[3];
00153     msg.angular.y = vel[4];
00154     msg.angular.z = vel[5];
00155     cmdvel.publish(msg);
00156   }
00157   else
00158   {
00159     throw vpRobotException (vpRobotException::wrongStateError,
00160                             "Cannot send the robot velocity in the specified control frame");
00161   }
00162 }
00163 
00164 
00175 void vpROSRobot::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &pose) {
00176   while(!odom_mutex);
00177   odom_mutex = false;
00178   if (frame == vpRobot::REFERENCE_FRAME)
00179   {
00180     pose.resize(6);
00181     pose[0] = p[0];
00182     pose[1] = p[1];
00183     pose[2] = p[2];
00184     vpRotationMatrix R(q);
00185     vpRxyzVector V(R);
00186     pose[3]=V[0];
00187     pose[4]=V[1];
00188     pose[5]=V[2];
00189   }
00190   else
00191   {
00192     throw vpRobotException (vpRobotException::wrongStateError,
00193                             "Cannot get the robot position in the specified control frame");
00194   }
00195   odom_mutex = true;
00196 }
00197 
00198 
00211 void vpROSRobot::getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &dis, struct timespec &timestamp){
00212   while(!odom_mutex);
00213   odom_mutex = false;
00214   vpColVector pose_cur(displacement);
00215   timestamp.tv_sec = _sec;
00216   timestamp.tv_nsec = _nsec;
00217   odom_mutex = true;
00218   if(frame == vpRobot::REFERENCE_FRAME){
00219     dis = pose_cur - pose_prev;
00220     pose_prev = pose_cur;
00221   }
00222   else
00223   {
00224     throw vpRobotException (vpRobotException::wrongStateError,
00225                             "Cannot get robot displacement in the specified control frame");
00226   }
00227 }
00228 
00239 void vpROSRobot::getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &dis){
00240   struct timespec timestamp;
00241   getDisplacement(frame, dis, timestamp);
00242 }
00243 
00244 
00245 void vpROSRobot::odomCallback(const nav_msgs::Odometry::ConstPtr& msg){
00246   while(!odom_mutex);
00247   odom_mutex = false;
00248   p.set(msg->pose.pose.position.x,msg->pose.pose.position.y,msg->pose.pose.position.z);
00249   q.set(msg->pose.pose.orientation.x,msg->pose.pose.orientation.y,msg->pose.pose.orientation.z,msg->pose.pose.orientation.w);
00250 
00251   if(_sec != 0 || _nsec != 0){
00252     double dt = ((double)msg->header.stamp.sec - (double)_sec) + ((double)msg->header.stamp.nsec - (double)_nsec) / 1000000000.0;
00253     displacement[0] += msg->twist.twist.linear.x * dt;
00254     displacement[1] += msg->twist.twist.linear.y * dt;
00255     displacement[2] += msg->twist.twist.linear.z * dt;
00256     displacement[3] += msg->twist.twist.angular.x * dt;
00257     displacement[4] += msg->twist.twist.angular.y * dt;
00258     displacement[5] += msg->twist.twist.angular.z * dt;
00259   }
00260   _sec = msg->header.stamp.sec;
00261   _nsec = msg->header.stamp.nsec;
00262   odom_mutex = true;
00263 }
00264 
00265 void vpROSRobot::stopMotion()
00266 {
00267   geometry_msgs::Twist msg;
00268   msg.linear.x = 0;
00269   msg.linear.y = 0;
00270   msg.linear.z = 0;
00271   msg.angular.x = 0;
00272   msg.angular.y = 0;
00273   msg.angular.z = 0;
00274   cmdvel.publish(msg);
00275 }
00276 
00277 
00285 void vpROSRobot::setCmdVelTopic(std::string topic_name)
00286 {
00287   _topic_cmd = topic_name;
00288 }
00289 
00290 
00298 void vpROSRobot::setOdomTopic(std::string topic_name)
00299 {
00300   _topic_odom = topic_name;
00301 }
00302 
00303 
00311 void vpROSRobot::setMasterURI(std::string master_uri)
00312 {
00313   _master_uri = master_uri;
00314 }
00315 
00323 void vpROSRobot::setNodespace(std::string nodespace)
00324 {
00325   _nodespace = nodespace;
00326 }


visp_ros
Author(s): Francois Pasteau, Fabien Spindler
autogenerated on Thu Mar 24 2016 03:32:49