vpROSRobot.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * $Id: vpROSRobot.cpp 3530 2012-01-03 10:52:12Z fpasteau $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2012 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * vpRobot implementation for ROS middleware
36  *
37  * Authors:
38  * Francois Pasteau
39  *
40  *****************************************************************************/
41 
42 
48 #include <visp/vpHomogeneousMatrix.h>
49 #include <visp/vpRobotException.h>
50 #include <visp_ros/vpROSRobot.h>
51 #include <visp/vpDebug.h>
52 #include <iostream>
53 #include <ros/ros.h>
54 #include <ros/time.h>
55 #include <sstream>
56 
59  isInitialized(false),
60  odom_mutex(true),
61  q(0,0,0,1),
62  p(0,0,0),
63  _sec(0),
64  _nsec(0),
65  displacement(6),
66  pose_prev(6),
67  _master_uri("http://127.0.0.1:11311"),
68  _topic_cmd("/RosAria/cmd_vel"),
69  _topic_odom("odom"),
70  _nodespace("")
71 {
72 
73 }
74 
75 
76 
79 {
80  if(isInitialized){
81  isInitialized = false;
82  spinner->stop();
83  delete spinner;
84  delete n;
85  }
86 }
87 
93 void vpROSRobot::init(int argc, char **argv)
94 {
95  std::cout << "ici 1\n";
96  if(!isInitialized){
97  if(!ros::isInitialized()) ros::init(argc, argv, "visp_node", ros::init_options::AnonymousName);
98  n = new ros::NodeHandle;
99  cmdvel = n->advertise<geometry_msgs::Twist>(_nodespace + _topic_cmd, 1);
101  spinner = new ros::AsyncSpinner(1);
102  spinner->start();
103  isInitialized = true;
104  std::cout << "ici 2\n";
105  }
106 }
107 
114  throw (vpRobotException(vpRobotException::constructionError,
115  "ROS robot already initialised with a different master_URI (" + ros::master::getURI() +" != " + _master_uri + ")") );
116  }
117  if(!isInitialized){
118  int argc = 2;
119  char *argv[2];
120  argv[0] = new char [255];
121  argv[1] = new char [255];
122 
123  std::string exe = "ros.exe", arg1 = "__master:=";
124  strcpy(argv[0], exe.c_str());
125  arg1.append(_master_uri);
126  strcpy(argv[1], arg1.c_str());
127  init(argc, argv);
128  delete [] argv[0];
129  delete [] argv[1];
130  }
131 }
132 
133 
144 void vpROSRobot::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
145 {
146  geometry_msgs::Twist msg;
147  if (frame == vpRobot::REFERENCE_FRAME)
148  {
149  msg.linear.x = vel[0];
150  msg.linear.y = vel[1];
151  msg.linear.z = vel[2];
152  msg.angular.x = vel[3];
153  msg.angular.y = vel[4];
154  msg.angular.z = vel[5];
155  cmdvel.publish(msg);
156  }
157  else
158  {
159  throw vpRobotException (vpRobotException::wrongStateError,
160  "Cannot send the robot velocity in the specified control frame");
161  }
162 }
163 
164 
175 void vpROSRobot::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &pose) {
176  while(!odom_mutex);
177  odom_mutex = false;
178  if (frame == vpRobot::REFERENCE_FRAME)
179  {
180  pose.resize(6);
181  pose[0] = p[0];
182  pose[1] = p[1];
183  pose[2] = p[2];
184  vpRotationMatrix R(q);
185  vpRxyzVector V(R);
186  pose[3]=V[0];
187  pose[4]=V[1];
188  pose[5]=V[2];
189  }
190  else
191  {
192  throw vpRobotException (vpRobotException::wrongStateError,
193  "Cannot get the robot position in the specified control frame");
194  }
195  odom_mutex = true;
196 }
197 
198 
211 void vpROSRobot::getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &dis, struct timespec &timestamp){
212  while(!odom_mutex);
213  odom_mutex = false;
214  vpColVector pose_cur(displacement);
215  timestamp.tv_sec = _sec;
216  timestamp.tv_nsec = _nsec;
217  odom_mutex = true;
218  if(frame == vpRobot::REFERENCE_FRAME){
219  dis = pose_cur - pose_prev;
220  pose_prev = pose_cur;
221  }
222  else
223  {
224  throw vpRobotException (vpRobotException::wrongStateError,
225  "Cannot get robot displacement in the specified control frame");
226  }
227 }
228 
239 void vpROSRobot::getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &dis){
240  struct timespec timestamp;
241  getDisplacement(frame, dis, timestamp);
242 }
243 
244 
245 void vpROSRobot::odomCallback(const nav_msgs::Odometry::ConstPtr& msg){
246  while(!odom_mutex);
247  odom_mutex = false;
248  p.set(msg->pose.pose.position.x,msg->pose.pose.position.y,msg->pose.pose.position.z);
249  q.set(msg->pose.pose.orientation.x,msg->pose.pose.orientation.y,msg->pose.pose.orientation.z,msg->pose.pose.orientation.w);
250 
251  if(_sec != 0 || _nsec != 0){
252  double dt = ((double)msg->header.stamp.sec - (double)_sec) + ((double)msg->header.stamp.nsec - (double)_nsec) / 1000000000.0;
253  displacement[0] += msg->twist.twist.linear.x * dt;
254  displacement[1] += msg->twist.twist.linear.y * dt;
255  displacement[2] += msg->twist.twist.linear.z * dt;
256  displacement[3] += msg->twist.twist.angular.x * dt;
257  displacement[4] += msg->twist.twist.angular.y * dt;
258  displacement[5] += msg->twist.twist.angular.z * dt;
259  }
260  _sec = msg->header.stamp.sec;
261  _nsec = msg->header.stamp.nsec;
262  odom_mutex = true;
263 }
264 
266 {
267  geometry_msgs::Twist msg;
268  msg.linear.x = 0;
269  msg.linear.y = 0;
270  msg.linear.z = 0;
271  msg.angular.x = 0;
272  msg.angular.y = 0;
273  msg.angular.z = 0;
274  cmdvel.publish(msg);
275 }
276 
277 
285 void vpROSRobot::setCmdVelTopic(std::string topic_name)
286 {
287  _topic_cmd = topic_name;
288 }
289 
290 
298 void vpROSRobot::setOdomTopic(std::string topic_name)
299 {
300  _topic_odom = topic_name;
301 }
302 
303 
311 void vpROSRobot::setMasterURI(std::string master_uri)
312 {
313  _master_uri = master_uri;
314 }
315 
323 void vpROSRobot::setNodespace(std::string nodespace)
324 {
325  _nodespace = nodespace;
326 }
vpROSRobot()
constructor
Definition: vpROSRobot.cpp:58
ROSCPP_DECL const std::string & getURI()
void setCmdVelTopic(std::string topic_name)
Definition: vpROSRobot.cpp:285
void set(const double x, const double y, const double z, const double w)
void getPosition(const vpRobot::vpControlFrameType, vpColVector &)
Definition: vpROSRobot.cpp:175
void publish(const boost::shared_ptr< M > &message) const
void odomCallback(const nav_msgs::Odometry::ConstPtr &msg)
Definition: vpROSRobot.cpp:245
volatile bool odom_mutex
Definition: vpROSRobot.h:77
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
vpColVector pose_prev
Definition: vpROSRobot.h:74
ros::AsyncSpinner * spinner
Definition: vpROSRobot.h:68
ROSCPP_DECL bool isInitialized()
ros::NodeHandle * n
Definition: vpROSRobot.h:65
std::string _topic_odom
Definition: vpROSRobot.h:80
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
vpRobot implementation for ROS middleware.
uint32_t _nsec
Definition: vpROSRobot.h:76
ros::Publisher cmdvel
Definition: vpROSRobot.h:66
void setNodespace(std::string nodespace)
Definition: vpROSRobot.cpp:323
void stopMotion()
Definition: vpROSRobot.cpp:265
virtual ~vpROSRobot()
destructor
Definition: vpROSRobot.cpp:78
ros::Subscriber odom
Definition: vpROSRobot.h:67
void setOdomTopic(std::string topic_name)
Definition: vpROSRobot.cpp:298
bool isInitialized
Definition: vpROSRobot.h:70
void init()
basic initialization
Definition: vpROSRobot.cpp:112
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void getDisplacement(const vpRobot::vpControlFrameType, vpColVector &)
Definition: vpROSRobot.cpp:239
std::string _master_uri
Definition: vpROSRobot.h:78
vpColVector displacement
Definition: vpROSRobot.h:75
std::string _topic_cmd
Definition: vpROSRobot.h:79
vpQuaternionVector q
Definition: vpROSRobot.h:72
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Definition: vpROSRobot.cpp:144
uint32_t _sec
Definition: vpROSRobot.h:76
void setMasterURI(std::string master_uri)
Definition: vpROSRobot.cpp:311
std::string _nodespace
Definition: vpROSRobot.h:81
vpTranslationVector p
Definition: vpROSRobot.h:73


visp_ros
Author(s): Francois Pasteau, Fabien Spindler, Gatien Gaumerais
autogenerated on Fri Jul 3 2020 03:41:43