afma6.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 <geometry_msgs/TwistStamped.h>
00007 #include <geometry_msgs/Pose.h>
00008 #include <geometry_msgs/PoseStamped.h>
00009 #include <nav_msgs/Odometry.h>
00010 #include <tf/tf.h>
00011 #include <tf/transform_listener.h>
00012 #include <tf/transform_broadcaster.h>
00013 #include <tf/transform_datatypes.h>
00014 
00015 #include <visp/vpRobotAfma6.h> // visp
00016 
00017 #include <visp_bridge/3dpose.h> // visp_bridge
00018 
00019 
00020 #ifdef VISP_HAVE_AFMA6
00021 
00022 class RosAfma6Node
00023 {
00024   public:
00025     RosAfma6Node(ros::NodeHandle n);
00026     virtual ~RosAfma6Node();
00027     
00028   public:
00029     int setup();
00030     void setCameraVel( const geometry_msgs::TwistStampedConstPtr &);
00031     void spin();
00032     void publish();
00033  
00034   protected:
00035     ros::NodeHandle n;
00036     ros::Publisher pose_pub;
00037     ros::Publisher vel_pub;
00038     ros::Subscriber cmd_camvel_sub;
00039 
00040     ros::Time veltime;
00041 
00042     std::string serial_port;
00043 
00044     vpRobotAfma6 *robot;
00045     geometry_msgs::PoseStamped position;
00046                 
00047     //for odom->base_link transform
00048     tf::TransformBroadcaster odom_broadcaster;
00049     geometry_msgs::TransformStamped odom_trans;
00050     //for resolving tf names.
00051     std::string tf_prefix;
00052     std::string frame_id_odom;
00053     std::string frame_id_base_link;
00054 
00055     vpHomogeneousMatrix wMc; // world to camera transformation
00056     vpColVector q; // measured joint position
00057  };
00058 
00059 
00060 RosAfma6Node::RosAfma6Node(ros::NodeHandle nh)
00061 {
00062   // read in config options
00063   n = nh;
00064 
00065   ROS_INFO( "using Afma6 robot" );
00066 
00067   robot = NULL;
00068   /*
00069    * Figure out what frame_id's to use. if a tf_prefix param is specified,
00070    * it will be added to the beginning of the frame_ids.
00071    *
00072    * e.g. rosrun ... _tf_prefix:=MyRobot (or equivalently using <param>s in
00073    * roslaunch files)
00074    * will result in the frame_ids being set to /MyRobot/odom etc,
00075    * rather than /odom. This is useful for Multi Robot Systems.
00076    * See ROS Wiki for further details.
00077    */
00078   tf_prefix = tf::getPrefixParam(n);
00079 //  frame_id_odom = tf::resolve(tf_prefix, "odom");
00080 //  frame_id_base_link = tf::resolve(tf_prefix, "base_link");
00081 
00082   // advertise services
00083   pose_pub = n.advertise<geometry_msgs::PoseStamped>("pose", 1000);
00084   vel_pub = n.advertise<geometry_msgs::TwistStamped>("velocity", 1000);
00085   
00086   // subscribe to services
00087   cmd_camvel_sub = n.subscribe( "cmd_camvel", 1, (boost::function < void(const geometry_msgs::TwistStampedConstPtr&)>) boost::bind( &RosAfma6Node::setCameraVel, this, _1 ));
00088 }
00089 
00090 RosAfma6Node::~RosAfma6Node()
00091 {
00092   if (robot) {
00093     robot->stopMotion();
00094     delete robot;
00095     robot = NULL;
00096   }
00097 }
00098 
00099 int RosAfma6Node::setup()
00100 {
00101   robot = new vpRobotAfma6;
00102 
00103   robot->init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithDistortion);
00104 
00105   robot->setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
00106 
00107   return 0;
00108 }
00109 
00110 void RosAfma6Node::spin()
00111 {
00112         ros::Rate loop_rate(100);
00113         while(ros::ok()){
00114                 this->publish();
00115                 ros::spinOnce();
00116                 loop_rate.sleep();
00117         }
00118 //  ros::spin();
00119 }
00120 
00121 void RosAfma6Node::publish()
00122 {
00123         double timestamp;
00124         robot->getPosition(vpRobot::ARTICULAR_FRAME, q, timestamp);
00125         wMc = robot->get_fMc(q);
00126         position.pose = visp_bridge::toGeometryMsgsPose(wMc);
00127         position.header.stamp = ros::Time(timestamp); // to improve: should be the timestamp returned by getPosition()
00128 
00129         //  ROS_INFO( "Afma6 publish pose at %f s: [%0.2f %0.2f %0.2f] - [%0.2f %0.2f %0.2f %0.2f]",
00130         //            position.header.stamp.toSec(),
00131         //            position.pose.position.x, position.pose.position.y, position.pose.position.z,
00132         //            position.pose.orientation.w, position.pose.orientation.x, position.pose.orientation.y, position.pose.orientation.z);
00133         pose_pub.publish(position);
00134 
00135         vpColVector vel(6);
00136         robot->getVelocity(vpRobot::CAMERA_FRAME, vel, timestamp);
00137         geometry_msgs::TwistStamped vel_msg;
00138         vel_msg.header.stamp = ros::Time(timestamp);
00139         vel_msg.twist.linear.x = vel[0];
00140         vel_msg.twist.linear.y = vel[1];
00141         vel_msg.twist.linear.z = vel[2];
00142         vel_msg.twist.angular.x = vel[3];
00143         vel_msg.twist.angular.y = vel[4];
00144         vel_msg.twist.angular.z = vel[5];
00145         vel_pub.publish(vel_msg);
00146 
00147 //      ros::Duration(1e-3).sleep();
00148 }
00149 
00150 void
00151 RosAfma6Node::setCameraVel( const geometry_msgs::TwistStampedConstPtr &msg)
00152 {
00153   veltime = ros::Time::now();
00154 
00155   vpColVector vc(6); // Vel in m/s and rad/s
00156 
00157   vc[0] = msg->twist.linear.x;
00158   vc[1] = msg->twist.linear.y;
00159   vc[2] = msg->twist.linear.z;
00160 
00161   vc[3] = msg->twist.angular.x;
00162   vc[4] = msg->twist.angular.y;
00163   vc[5] = msg->twist.angular.z;
00164 
00165 //  ROS_INFO( "Afma6 new camera vel at %f s: [%0.2f %0.2f %0.2f] m/s [%0.2f %0.2f %0.2f] rad/s",
00166 //            veltime.toSec(),
00167 //            vc[0], vc[1], vc[2], vc[3], vc[4], vc[5]);
00168   robot->setVelocity(vpRobot::CAMERA_FRAME, vc);
00169 
00170 //  this->publish();
00171 }
00172 
00173 #endif // #ifdef VISP_HAVE_AFMA6
00174 
00175 int main( int argc, char** argv )
00176 {
00177 #ifdef VISP_HAVE_AFMA6
00178   ros::init(argc,argv, "RosAfma6");
00179   ros::NodeHandle n(std::string("~"));
00180 
00181   RosAfma6Node *node = new RosAfma6Node(n);
00182 
00183   if( node->setup() != 0 )
00184   {
00185     printf( "Afma6 setup failed... \n" );
00186     return -1;
00187   }
00188 
00189   node->spin();
00190 
00191   delete node;
00192 
00193   printf( "\nQuitting... \n" );
00194 #else
00195   printf("This node is node available since ViSP was \nnot build with Afma6 robot support...\n");
00196 #endif
00197   return 0;
00198 }
00199 


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