viper650.cpp
Go to the documentation of this file.
00001 #include <stdio.h>
00002 #include <math.h>
00003 #include <sstream>
00004 
00005 #include <ros/ros.h>
00006 #include <std_msgs/Float64MultiArray.h>
00007 #include <sensor_msgs/JointState.h>
00008 #include <geometry_msgs/TwistStamped.h>
00009 #include <geometry_msgs/Pose.h>
00010 #include <geometry_msgs/PoseStamped.h>
00011 #include <nav_msgs/Odometry.h>
00012 #include <tf/tf.h>
00013 #include <tf/transform_listener.h>
00014 #include <tf/transform_broadcaster.h>
00015 #include <tf/transform_datatypes.h>
00016 
00017 #include <visp3/core/vpMath.h>
00018 #include <visp3/core/vpIoTools.h>
00019 #include <visp3/core/vpHomogeneousMatrix.h>
00020 #include <visp3/robot/vpRobotViper650.h>
00021 
00022 #include <visp_bridge/3dpose.h> // visp_bridge
00023 
00024 
00025 #ifdef VISP_HAVE_VIPER650
00026 
00027 class RosViper650Node
00028 {
00029   public:
00030     RosViper650Node(ros::NodeHandle n);
00031     virtual ~RosViper650Node();
00032     
00033   public:
00034     int setup();
00035     void setCameraVel(const geometry_msgs::TwistStampedConstPtr &);
00036     void setJointVel(const sensor_msgs::JointStateConstPtr &);
00037     void setRefVel(const geometry_msgs::TwistStampedConstPtr &);
00038     void spin();
00039     void publish();
00040  
00041   protected:
00042     ros::NodeHandle n;
00043     ros::Publisher pose_pub;
00044     ros::Publisher vel_pub;
00045     ros::Publisher jointState_pub;
00046     ros::Publisher jacobian_pub;
00047     ros::Subscriber cmd_camvel_sub;
00048     ros::Subscriber cmd_jointvel_sub;
00049     ros::Subscriber cmd_refvel_sub;
00050     std::string setControlMode; // "joint_space", "camera_frame" (default), "reference_frame"
00051     std::string getStateSpace;  // "joint_space", "camera_frame" (default), "reference_frame"
00052     std::string cmdVelTopicName;  // default to /cmd_vel
00053     std::string endEffectorType;  // "TOOL_PTGREY_FLEA2_CAMERA" (default), "TOOL_GENERIC_CAMERA", "TOOL_CUSTOM"
00054     std::string customToolTransformationFileName;
00055 
00056     ros::Time veltime;
00057 
00058     std::string serial_port;
00059 
00060     vpRobotViper650 *robot;
00061     geometry_msgs::PoseStamped position;
00062     sensor_msgs::JointState jointState;
00063     std_msgs::Float64MultiArray jacobian;
00064                 
00065     //for odom->base_link transform
00066     tf::TransformBroadcaster odom_broadcaster;
00067     geometry_msgs::TransformStamped odom_trans;
00068     //for resolving tf names.
00069     std::string tf_prefix;
00070     std::string frame_id_odom;
00071     std::string frame_id_base_link;
00072 
00073     vpHomogeneousMatrix wMc; // world to camera transformation
00074     vpColVector q; // measured joint position
00075  };
00076 
00077 
00078 RosViper650Node::RosViper650Node(ros::NodeHandle nh)
00079 {
00080   // read in config options
00081   n = nh;
00082 
00083   ROS_INFO("Launch Viper650 robot ros node");
00084 
00085   robot = NULL;
00086   n.param<std::string>("set_control_mode", setControlMode, "tool_frame");
00087   n.param<std::string>("get_state_space", getStateSpace, "tool_frame");
00088   n.param<std::string>("cmd_vel_topic_name", cmdVelTopicName, "cmd_vel");
00089   n.param<std::string>("end_effector", endEffectorType, "TOOL_PTGREY_FLEA2_CAMERA");
00090   n.param<std::string>("custom_tool_transformation_filename", customToolTransformationFileName, "");
00091 
00092   /*
00093    * Figure out what frame_id's to use. if a tf_prefix param is specified,
00094    * it will be added to the beginning of the frame_ids.
00095    *
00096    * e.g. rosrun ... _tf_prefix:=MyRobot (or equivalently using <param>s in
00097    * roslaunch files)
00098    * will result in the frame_ids being set to /MyRobot/odom etc,
00099    * rather than /odom. This is useful for Multi Robot Systems.
00100    * See ROS Wiki for further details.
00101    */
00102   tf_prefix = tf::getPrefixParam(n);
00103 //  frame_id_odom = tf::resolve(tf_prefix, "odom");
00104 //  frame_id_base_link = tf::resolve(tf_prefix, "base_link");
00105 
00106   // advertise services
00107 //  pose_pub = n.advertise<geometry_msgs::PoseStamped>("pose", 1000);
00108 //  vel_pub = n.advertise<geometry_msgs::TwistStamped>("velocity", 1000);
00109 //  jointState_pub = n.advertise<sensor_msgs::JointState>("joint_state", 1000);
00110 
00111   
00112   // subscribe to services
00113   if (setControlMode == "joint_space") {
00114     ROS_INFO("Viper650 robot controlled in joint space");
00115     jointState_pub = n.advertise<sensor_msgs::JointState>("joint_state", 10);
00116     jacobian_pub = n.advertise<std_msgs::Float64MultiArray>("jacobian", 10);
00117     cmd_jointvel_sub = n.subscribe( cmdVelTopicName, 1, (boost::function < void(const sensor_msgs::JointStateConstPtr&)>) boost::bind( &RosViper650Node::setJointVel, this, _1 ));
00118   }
00119   else if (setControlMode == "camera_frame") {
00120     ROS_INFO("Viper650 robot controlled in camera frame");
00121     pose_pub = n.advertise<geometry_msgs::PoseStamped>("pose", 10);
00122     vel_pub = n.advertise<geometry_msgs::TwistStamped>("velocity", 10);
00123     cmd_camvel_sub = n.subscribe( cmdVelTopicName, 1, (boost::function < void(const geometry_msgs::TwistStampedConstPtr&)>) boost::bind( &RosViper650Node::setCameraVel, this, _1 ));
00124   }
00125   else if (setControlMode == "reference_frame") {
00126     ROS_INFO("Viper650 robot controlled in reference frame");
00127     pose_pub = n.advertise<geometry_msgs::PoseStamped>("pose", 10);
00128     vel_pub = n.advertise<geometry_msgs::TwistStamped>("velocity", 10);
00129     cmd_refvel_sub = n.subscribe( cmdVelTopicName, 1, (boost::function < void(const geometry_msgs::TwistStampedConstPtr&)>) boost::bind( &RosViper650Node::setRefVel, this, _1 ));
00130   }
00131 }
00132 
00133 RosViper650Node::~RosViper650Node()
00134 {
00135   if (robot) {
00136     robot->stopMotion();
00137     delete robot;
00138     robot = NULL;
00139   }
00140 }
00141 
00142 int RosViper650Node::setup()
00143 {
00144   robot = new vpRobotViper650;
00145 
00146   if (endEffectorType == "TOOL_PTGREY_FLEA2_CAMERA") {
00147     robot->init(vpViper650::TOOL_PTGREY_FLEA2_CAMERA, vpCameraParameters::perspectiveProjWithDistortion);
00148   }
00149   else if (endEffectorType == "TOOL_GENERIC_CAMERA") {
00150     robot->init(vpViper650::TOOL_GENERIC_CAMERA, vpCameraParameters::perspectiveProjWithDistortion);
00151   }
00152   else if (endEffectorType == "TOOL_CUSTOM") {
00153     if (vpIoTools::checkFilename(customToolTransformationFileName) == false) {
00154       ROS_ERROR("Viper650: Missing or bad filename for eMt custom tool transformation");
00155       return -1;
00156     }
00157 
00158     vpHomogeneousMatrix eMt;
00159     std::ifstream f(customToolTransformationFileName.c_str());
00160     try {
00161       eMt.load(f);
00162       f.close();
00163     }
00164     catch(vpException &e) {
00165       ROS_ERROR_STREAM("Viper650: Cannot load eMt custom tool transformation from \"" << customToolTransformationFileName << "\": " << e.getStringMessage());
00166       f.close();
00167       return -1;
00168     }
00169 
00170     robot->init(vpViper650::TOOL_CUSTOM, eMt);
00171   }
00172 
00173   robot->setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
00174 
00175   return 0;
00176 }
00177 
00178 void RosViper650Node::spin()
00179 {
00180         ros::Rate loop_rate(100);
00181         while(ros::ok()){
00182                 this->publish();
00183                 ros::spinOnce();
00184                 loop_rate.sleep();
00185         }
00186 }
00187 
00188 void RosViper650Node::publish()
00189 {
00190         double timestamp;
00191         robot->getPosition(vpRobot::ARTICULAR_FRAME, q, timestamp);
00192 
00193   if (getStateSpace == "joint_space") {
00194     vpColVector qdot;
00195     robot->getVelocity(vpRobot::ARTICULAR_FRAME, qdot, timestamp);
00196     jointState.position.clear();
00197     jointState.velocity.clear();
00198     jointState.header.stamp = ros::Time(timestamp);
00199     for (unsigned int i=0; i<qdot.size(); i++) {
00200       jointState.position.push_back(q[i]);
00201       jointState.velocity.push_back(qdot[i]);
00202     }
00203     jointState_pub.publish(jointState);
00204 
00205     vpMatrix eJe;
00206     robot->get_eJe(eJe);
00207     vpHomogeneousMatrix eMc;
00208     robot->get_eMc(eMc);
00209     vpMatrix cJc = vpVelocityTwistMatrix(eMc.inverse()) * eJe;
00210 
00211     jacobian.data.clear();
00212     jacobian.layout.dim.resize(2);
00213 
00214     jacobian.layout.dim[0].label = "height";
00215     jacobian.layout.dim[0].size = cJc.getRows();
00216     jacobian.layout.dim[0].stride = cJc.size();
00217     jacobian.layout.dim[1].label = "width";
00218     jacobian.layout.dim[1].size = cJc.getCols();
00219     jacobian.layout.dim[1].stride = cJc.getCols();
00220     for(unsigned int i =0; i< cJc.size(); i++)
00221       jacobian.data.push_back( cJc.data[i]);
00222     jacobian_pub.publish(jacobian);
00223   }
00224   else if (getStateSpace == "camera_frame") {
00225     wMc = robot->get_fMc(q);
00226     position.pose = visp_bridge::toGeometryMsgsPose(wMc);
00227     position.header.stamp = ros::Time(timestamp); // to improve: should be the timestamp returned by getPosition()
00228 
00229     //  ROS_INFO( "Viper650 publish pose at %f s: [%0.2f %0.2f %0.2f] - [%0.2f %0.2f %0.2f %0.2f]",
00230     //            position.header.stamp.toSec(),
00231     //            position.pose.position.x, position.pose.position.y, position.pose.position.z,
00232     //            position.pose.orientation.w, position.pose.orientation.x, position.pose.orientation.y, position.pose.orientation.z);
00233     pose_pub.publish(position);
00234 
00235     vpColVector vel(6);
00236     robot->getVelocity(vpRobot::CAMERA_FRAME, vel, timestamp);
00237     geometry_msgs::TwistStamped vel_msg;
00238     vel_msg.header.stamp = ros::Time(timestamp);
00239     vel_msg.twist.linear.x = vel[0];
00240     vel_msg.twist.linear.y = vel[1];
00241     vel_msg.twist.linear.z = vel[2];
00242     vel_msg.twist.angular.x = vel[3];
00243     vel_msg.twist.angular.y = vel[4];
00244     vel_msg.twist.angular.z = vel[5];
00245     vel_pub.publish(vel_msg);
00246   }
00247   else if (getStateSpace == "reference_frame") {
00248     wMc = robot->get_fMc(q);
00249     position.pose = visp_bridge::toGeometryMsgsPose(wMc);
00250     position.header.stamp = ros::Time(timestamp); // to improve: should be the timestamp returned by getPosition()
00251 
00252     //  ROS_INFO( "Viper650 publish pose at %f s: [%0.2f %0.2f %0.2f] - [%0.2f %0.2f %0.2f %0.2f]",
00253     //            position.header.stamp.toSec(),
00254     //            position.pose.position.x, position.pose.position.y, position.pose.position.z,
00255     //            position.pose.orientation.w, position.pose.orientation.x, position.pose.orientation.y, position.pose.orientation.z);
00256     pose_pub.publish(position);
00257 
00258     vpColVector vel(6);
00259     robot->getVelocity(vpRobot::REFERENCE_FRAME, vel, timestamp);
00260     geometry_msgs::TwistStamped vel_msg;
00261     vel_msg.header.stamp = ros::Time(timestamp);
00262     vel_msg.twist.linear.x = vel[0];
00263     vel_msg.twist.linear.y = vel[1];
00264     vel_msg.twist.linear.z = vel[2];
00265     vel_msg.twist.angular.x = vel[3];
00266     vel_msg.twist.angular.y = vel[4];
00267     vel_msg.twist.angular.z = vel[5];
00268     vel_pub.publish(vel_msg);
00269   }
00270 }
00271 
00272 void
00273 RosViper650Node::setCameraVel(const geometry_msgs::TwistStampedConstPtr &msg)
00274 {
00275   veltime = ros::Time::now();
00276 
00277   vpColVector vc(6); // Vel in m/s and rad/s
00278 
00279   vc[0] = msg->twist.linear.x;
00280   vc[1] = msg->twist.linear.y;
00281   vc[2] = msg->twist.linear.z;
00282 
00283   vc[3] = msg->twist.angular.x;
00284   vc[4] = msg->twist.angular.y;
00285   vc[5] = msg->twist.angular.z;
00286 
00287   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",
00288             veltime.toSec(),
00289             vc[0], vc[1], vc[2], vc[3], vc[4], vc[5]);
00290   robot->setVelocity(vpRobot::CAMERA_FRAME, vc);
00291 }
00292 
00293 void
00294 RosViper650Node::setJointVel(const sensor_msgs::JointStateConstPtr &msg)
00295 {
00296   veltime = ros::Time::now();
00297 
00298   if (msg->velocity.size() != 6) {
00299     ROS_ERROR("Viper650: Cannot apply a joint velocity vector that is not 6 dimensional");
00300     return;
00301   }
00302   vpColVector qdot(6); // Vel in rad/s for each joint
00303 
00304   for(unsigned int i=0; i<msg->velocity.size(); i++)
00305     qdot[i] = msg->velocity[i];
00306 
00307   ROS_INFO("Viper650 new joint vel at %f s: [%0.2f %0.2f %0.2f %0.2f %0.2f %0.2f] rad/s",
00308            veltime.toSec(),
00309            qdot[0], qdot[1], qdot[2], qdot[3], qdot[4], qdot[5]);
00310 
00311   robot->setVelocity(vpRobot::ARTICULAR_FRAME, qdot);
00312 }
00313 
00314 void
00315 RosViper650Node::setRefVel(const geometry_msgs::TwistStampedConstPtr &msg)
00316 {
00317   veltime = ros::Time::now();
00318 
00319   vpColVector vref(6); // Vel in m/s and rad/s
00320 
00321   vref[0] = msg->twist.linear.x;
00322   vref[1] = msg->twist.linear.y;
00323   vref[2] = msg->twist.linear.z;
00324 
00325   vref[3] = msg->twist.angular.x;
00326   vref[4] = msg->twist.angular.y;
00327   vref[5] = msg->twist.angular.z;
00328 
00329   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",
00330             veltime.toSec(),
00331             vref[0], vref[1], vref[2], vref[3], vref[4], vref[5]);
00332   robot->setVelocity(vpRobot::REFERENCE_FRAME, vref);
00333 }
00334 
00335 #endif // #ifdef VISP_HAVE_Viper650
00336 
00337 int main( int argc, char** argv )
00338 {
00339 #ifdef VISP_HAVE_VIPER650
00340   ros::init(argc,argv, "RosViper650");
00341   ros::NodeHandle n(std::string("~"));
00342 
00343   RosViper650Node *node = new RosViper650Node(n);
00344 
00345   if( node->setup() != 0 )
00346   {
00347     printf( "Viper650 setup failed... \n" );
00348     return -1;
00349   }
00350 
00351   node->spin();
00352 
00353   delete node;
00354 
00355   printf( "\nQuitting... \n" );
00356 #else
00357   printf("The Viper650 robot is not supported by ViSP...\n");
00358 #endif
00359   return 0;
00360 }
00361 


visp_ros
Author(s): Francois Pasteau, Fabien Spindler
autogenerated on Thu Jun 6 2019 18:06:25