viper650.cpp
Go to the documentation of this file.
1 #include <stdio.h>
2 #include <math.h>
3 #include <sstream>
4 
5 #include <ros/ros.h>
6 #include <std_msgs/Float64MultiArray.h>
7 #include <sensor_msgs/JointState.h>
8 #include <geometry_msgs/TwistStamped.h>
9 #include <geometry_msgs/Pose.h>
10 #include <geometry_msgs/PoseStamped.h>
11 #include <nav_msgs/Odometry.h>
12 #include <tf/tf.h>
13 #include <tf/transform_listener.h>
15 #include <tf/transform_datatypes.h>
16 
17 #include <visp3/core/vpMath.h>
18 #include <visp3/core/vpIoTools.h>
19 #include <visp3/core/vpHomogeneousMatrix.h>
20 #include <visp3/robot/vpRobotViper650.h>
21 
22 #include <visp_bridge/3dpose.h> // visp_bridge
23 
24 
25 #ifdef VISP_HAVE_VIPER650
26 
27 class RosViper650Node
28 {
29  public:
30  RosViper650Node(ros::NodeHandle n);
31  virtual ~RosViper650Node();
32 
33  public:
34  int setup();
35  void setCameraVel(const geometry_msgs::TwistStampedConstPtr &);
36  void setJointVel(const sensor_msgs::JointStateConstPtr &);
37  void setRefVel(const geometry_msgs::TwistStampedConstPtr &);
38  void spin();
39  void publish();
40 
41  protected:
43  ros::Publisher pose_pub;
44  ros::Publisher vel_pub;
45  ros::Publisher jointState_pub;
46  ros::Publisher jacobian_pub;
47  ros::Subscriber cmd_camvel_sub;
48  ros::Subscriber cmd_jointvel_sub;
49  ros::Subscriber cmd_refvel_sub;
50  std::string setControlMode; // "joint_space", "camera_frame" (default), "reference_frame"
51  std::string getStateSpace; // "joint_space", "camera_frame" (default), "reference_frame"
52  std::string cmdVelTopicName; // default to /cmd_vel
53  std::string endEffectorType; // "TOOL_PTGREY_FLEA2_CAMERA" (default), "TOOL_GENERIC_CAMERA", "TOOL_CUSTOM"
54  std::string customToolTransformationFileName;
55 
56  ros::Time veltime;
57 
58  std::string serial_port;
59 
60  vpRobotViper650 *robot;
61  geometry_msgs::PoseStamped position;
62  sensor_msgs::JointState jointState;
63  std_msgs::Float64MultiArray jacobian;
64 
65  //for odom->base_link transform
66  tf::TransformBroadcaster odom_broadcaster;
67  geometry_msgs::TransformStamped odom_trans;
68  //for resolving tf names.
69  std::string tf_prefix;
70  std::string frame_id_odom;
71  std::string frame_id_base_link;
72 
73  vpHomogeneousMatrix wMc; // world to camera transformation
74  vpColVector q; // measured joint position
75  };
76 
77 
78 RosViper650Node::RosViper650Node(ros::NodeHandle nh)
79 {
80  // read in config options
81  n = nh;
82 
83  ROS_INFO("Launch Viper650 robot ros node");
84 
85  robot = NULL;
86  n.param<std::string>("set_control_mode", setControlMode, "tool_frame");
87  n.param<std::string>("get_state_space", getStateSpace, "tool_frame");
88  n.param<std::string>("cmd_vel_topic_name", cmdVelTopicName, "cmd_vel");
89  n.param<std::string>("end_effector", endEffectorType, "TOOL_PTGREY_FLEA2_CAMERA");
90  n.param<std::string>("custom_tool_transformation_filename", customToolTransformationFileName, "");
91 
92  /*
93  * Figure out what frame_id's to use. if a tf_prefix param is specified,
94  * it will be added to the beginning of the frame_ids.
95  *
96  * e.g. rosrun ... _tf_prefix:=MyRobot (or equivalently using <param>s in
97  * roslaunch files)
98  * will result in the frame_ids being set to /MyRobot/odom etc,
99  * rather than /odom. This is useful for Multi Robot Systems.
100  * See ROS Wiki for further details.
101  */
102  tf_prefix = tf::getPrefixParam(n);
103 // frame_id_odom = tf::resolve(tf_prefix, "odom");
104 // frame_id_base_link = tf::resolve(tf_prefix, "base_link");
105 
106  // advertise services
107 // pose_pub = n.advertise<geometry_msgs::PoseStamped>("pose", 1000);
108 // vel_pub = n.advertise<geometry_msgs::TwistStamped>("velocity", 1000);
109 // jointState_pub = n.advertise<sensor_msgs::JointState>("joint_state", 1000);
110 
111 
112  // subscribe to services
113  if (setControlMode == "joint_space") {
114  ROS_INFO("Viper650 robot controlled in joint space");
115  jointState_pub = n.advertise<sensor_msgs::JointState>("joint_state", 10);
116  jacobian_pub = n.advertise<std_msgs::Float64MultiArray>("jacobian", 10);
117  cmd_jointvel_sub = n.subscribe( cmdVelTopicName, 1, (boost::function < void(const sensor_msgs::JointStateConstPtr&)>) boost::bind( &RosViper650Node::setJointVel, this, _1 ));
118  }
119  else if (setControlMode == "camera_frame") {
120  ROS_INFO("Viper650 robot controlled in camera frame");
121  pose_pub = n.advertise<geometry_msgs::PoseStamped>("pose", 10);
122  vel_pub = n.advertise<geometry_msgs::TwistStamped>("velocity", 10);
123  cmd_camvel_sub = n.subscribe( cmdVelTopicName, 1, (boost::function < void(const geometry_msgs::TwistStampedConstPtr&)>) boost::bind( &RosViper650Node::setCameraVel, this, _1 ));
124  }
125  else if (setControlMode == "reference_frame") {
126  ROS_INFO("Viper650 robot controlled in reference frame");
127  pose_pub = n.advertise<geometry_msgs::PoseStamped>("pose", 10);
128  vel_pub = n.advertise<geometry_msgs::TwistStamped>("velocity", 10);
129  cmd_refvel_sub = n.subscribe( cmdVelTopicName, 1, (boost::function < void(const geometry_msgs::TwistStampedConstPtr&)>) boost::bind( &RosViper650Node::setRefVel, this, _1 ));
130  }
131 }
132 
133 RosViper650Node::~RosViper650Node()
134 {
135  if (robot) {
136  robot->stopMotion();
137  delete robot;
138  robot = NULL;
139  }
140 }
141 
142 int RosViper650Node::setup()
143 {
144  robot = new vpRobotViper650;
145 
146  if (endEffectorType == "TOOL_PTGREY_FLEA2_CAMERA") {
147  robot->init(vpViper650::TOOL_PTGREY_FLEA2_CAMERA, vpCameraParameters::perspectiveProjWithDistortion);
148  }
149  else if (endEffectorType == "TOOL_GENERIC_CAMERA") {
150  robot->init(vpViper650::TOOL_GENERIC_CAMERA, vpCameraParameters::perspectiveProjWithDistortion);
151  }
152  else if (endEffectorType == "TOOL_CUSTOM") {
153  if (vpIoTools::checkFilename(customToolTransformationFileName) == false) {
154  ROS_ERROR("Viper650: Missing or bad filename for eMt custom tool transformation");
155  return -1;
156  }
157 
158  vpHomogeneousMatrix eMt;
159  std::ifstream f(customToolTransformationFileName.c_str());
160  try {
161  eMt.load(f);
162  f.close();
163  }
164  catch(vpException &e) {
165  ROS_ERROR_STREAM("Viper650: Cannot load eMt custom tool transformation from \"" << customToolTransformationFileName << "\": " << e.getStringMessage());
166  f.close();
167  return -1;
168  }
169 
170  robot->init(vpViper650::TOOL_CUSTOM, eMt);
171  }
172 
173  robot->setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
174 
175  return 0;
176 }
177 
178 void RosViper650Node::spin()
179 {
180  ros::Rate loop_rate(100);
181  while(ros::ok()){
182  this->publish();
183  ros::spinOnce();
184  loop_rate.sleep();
185  }
186 }
187 
188 void RosViper650Node::publish()
189 {
190  double timestamp;
191  robot->getPosition(vpRobot::ARTICULAR_FRAME, q, timestamp);
192 
193  if (getStateSpace == "joint_space") {
194  vpColVector qdot;
195  robot->getVelocity(vpRobot::ARTICULAR_FRAME, qdot, timestamp);
196  jointState.position.clear();
197  jointState.velocity.clear();
198  jointState.header.stamp = ros::Time(timestamp);
199  for (unsigned int i=0; i<qdot.size(); i++) {
200  jointState.position.push_back(q[i]);
201  jointState.velocity.push_back(qdot[i]);
202  }
203  jointState_pub.publish(jointState);
204 
205  vpMatrix eJe;
206  robot->get_eJe(eJe);
207  vpHomogeneousMatrix eMc;
208  robot->get_eMc(eMc);
209  vpMatrix cJc = vpVelocityTwistMatrix(eMc.inverse()) * eJe;
210 
211  jacobian.data.clear();
212  jacobian.layout.dim.resize(2);
213 
214  jacobian.layout.dim[0].label = "height";
215  jacobian.layout.dim[0].size = cJc.getRows();
216  jacobian.layout.dim[0].stride = cJc.size();
217  jacobian.layout.dim[1].label = "width";
218  jacobian.layout.dim[1].size = cJc.getCols();
219  jacobian.layout.dim[1].stride = cJc.getCols();
220  for(unsigned int i =0; i< cJc.size(); i++)
221  jacobian.data.push_back( cJc.data[i]);
222  jacobian_pub.publish(jacobian);
223  }
224  else if (getStateSpace == "camera_frame") {
225  wMc = robot->get_fMc(q);
226  position.pose = visp_bridge::toGeometryMsgsPose(wMc);
227  position.header.stamp = ros::Time(timestamp); // to improve: should be the timestamp returned by getPosition()
228 
229  // ROS_INFO( "Viper650 publish pose at %f s: [%0.2f %0.2f %0.2f] - [%0.2f %0.2f %0.2f %0.2f]",
230  // position.header.stamp.toSec(),
231  // position.pose.position.x, position.pose.position.y, position.pose.position.z,
232  // position.pose.orientation.w, position.pose.orientation.x, position.pose.orientation.y, position.pose.orientation.z);
233  pose_pub.publish(position);
234 
235  vpColVector vel(6);
236  robot->getVelocity(vpRobot::CAMERA_FRAME, vel, timestamp);
237  geometry_msgs::TwistStamped vel_msg;
238  vel_msg.header.stamp = ros::Time(timestamp);
239  vel_msg.twist.linear.x = vel[0];
240  vel_msg.twist.linear.y = vel[1];
241  vel_msg.twist.linear.z = vel[2];
242  vel_msg.twist.angular.x = vel[3];
243  vel_msg.twist.angular.y = vel[4];
244  vel_msg.twist.angular.z = vel[5];
245  vel_pub.publish(vel_msg);
246  }
247  else if (getStateSpace == "reference_frame") {
248  wMc = robot->get_fMc(q);
249  position.pose = visp_bridge::toGeometryMsgsPose(wMc);
250  position.header.stamp = ros::Time(timestamp); // to improve: should be the timestamp returned by getPosition()
251 
252  // ROS_INFO( "Viper650 publish pose at %f s: [%0.2f %0.2f %0.2f] - [%0.2f %0.2f %0.2f %0.2f]",
253  // position.header.stamp.toSec(),
254  // position.pose.position.x, position.pose.position.y, position.pose.position.z,
255  // position.pose.orientation.w, position.pose.orientation.x, position.pose.orientation.y, position.pose.orientation.z);
256  pose_pub.publish(position);
257 
258  vpColVector vel(6);
259  robot->getVelocity(vpRobot::REFERENCE_FRAME, vel, timestamp);
260  geometry_msgs::TwistStamped vel_msg;
261  vel_msg.header.stamp = ros::Time(timestamp);
262  vel_msg.twist.linear.x = vel[0];
263  vel_msg.twist.linear.y = vel[1];
264  vel_msg.twist.linear.z = vel[2];
265  vel_msg.twist.angular.x = vel[3];
266  vel_msg.twist.angular.y = vel[4];
267  vel_msg.twist.angular.z = vel[5];
268  vel_pub.publish(vel_msg);
269  }
270 }
271 
272 void
273 RosViper650Node::setCameraVel(const geometry_msgs::TwistStampedConstPtr &msg)
274 {
275  veltime = ros::Time::now();
276 
277  vpColVector vc(6); // Vel in m/s and rad/s
278 
279  vc[0] = msg->twist.linear.x;
280  vc[1] = msg->twist.linear.y;
281  vc[2] = msg->twist.linear.z;
282 
283  vc[3] = msg->twist.angular.x;
284  vc[4] = msg->twist.angular.y;
285  vc[5] = msg->twist.angular.z;
286 
287  ROS_INFO( "Viper650 new camera vel at %f s: [%0.2f %0.2f %0.2f] m/s [%0.2f %0.2f %0.2f] rad/s",
288  veltime.toSec(),
289  vc[0], vc[1], vc[2], vc[3], vc[4], vc[5]);
290  robot->setVelocity(vpRobot::CAMERA_FRAME, vc);
291 }
292 
293 void
294 RosViper650Node::setJointVel(const sensor_msgs::JointStateConstPtr &msg)
295 {
296  veltime = ros::Time::now();
297 
298  if (msg->velocity.size() != 6) {
299  ROS_ERROR("Viper650: Cannot apply a joint velocity vector that is not 6 dimensional");
300  return;
301  }
302  vpColVector qdot(6); // Vel in rad/s for each joint
303 
304  for(unsigned int i=0; i<msg->velocity.size(); i++)
305  qdot[i] = msg->velocity[i];
306 
307  ROS_INFO("Viper650 new joint vel at %f s: [%0.2f %0.2f %0.2f %0.2f %0.2f %0.2f] rad/s",
308  veltime.toSec(),
309  qdot[0], qdot[1], qdot[2], qdot[3], qdot[4], qdot[5]);
310 
311  robot->setVelocity(vpRobot::ARTICULAR_FRAME, qdot);
312 }
313 
314 void
315 RosViper650Node::setRefVel(const geometry_msgs::TwistStampedConstPtr &msg)
316 {
317  veltime = ros::Time::now();
318 
319  vpColVector vref(6); // Vel in m/s and rad/s
320 
321  vref[0] = msg->twist.linear.x;
322  vref[1] = msg->twist.linear.y;
323  vref[2] = msg->twist.linear.z;
324 
325  vref[3] = msg->twist.angular.x;
326  vref[4] = msg->twist.angular.y;
327  vref[5] = msg->twist.angular.z;
328 
329  ROS_INFO( "Viper650 new reference vel at %f s: [%0.2f %0.2f %0.2f] m/s [%0.2f %0.2f %0.2f] rad/s",
330  veltime.toSec(),
331  vref[0], vref[1], vref[2], vref[3], vref[4], vref[5]);
332  robot->setVelocity(vpRobot::REFERENCE_FRAME, vref);
333 }
334 
335 #endif // #ifdef VISP_HAVE_Viper650
336 
337 int main( int argc, char** argv )
338 {
339 #ifdef VISP_HAVE_VIPER650
340  ros::init(argc,argv, "RosViper650");
341  ros::NodeHandle n(std::string("~"));
342 
343  RosViper650Node *node = new RosViper650Node(n);
344 
345  if( node->setup() != 0 )
346  {
347  printf( "Viper650 setup failed... \n" );
348  return -1;
349  }
350 
351  node->spin();
352 
353  delete node;
354 
355  printf( "\nQuitting... \n" );
356 #else
357  printf("The Viper650 robot is not supported by ViSP...\n");
358 #endif
359  return 0;
360 }
361 
std::string getPrefixParam(ros::NodeHandle &nh)
f
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL void spin(Spinner &spinner)
geometry_msgs::Pose toGeometryMsgsPose(const vpHomogeneousMatrix &mat)
int main(int argc, char **argv)
Definition: viper650.cpp:337
#define ROS_INFO(...)
ROSCPP_DECL bool ok()
static Time now()
#define ROS_ERROR_STREAM(args)
ROSCPP_DECL void spinOnce()
#define ROS_ERROR(...)


visp_ros
Author(s): Francois Pasteau, Fabien Spindler, Gatien Gaumerais
autogenerated on Tue Feb 9 2021 03:40:20