afma6.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 <geometry_msgs/TwistStamped.h>
7 #include <geometry_msgs/Pose.h>
8 #include <geometry_msgs/PoseStamped.h>
9 #include <nav_msgs/Odometry.h>
10 #include <tf/tf.h>
11 #include <tf/transform_listener.h>
13 #include <tf/transform_datatypes.h>
14 
15 #include <visp/vpRobotAfma6.h> // visp
16 
17 #include <visp_bridge/3dpose.h> // visp_bridge
18 
19 
20 #ifdef VISP_HAVE_AFMA6
21 
23 {
24  public:
26  virtual ~RosAfma6Node();
27 
28  public:
29  int setup();
30  void setCameraVel( const geometry_msgs::TwistStampedConstPtr &);
31  void spin();
32  void publish();
33 
34  protected:
39 
41 
42  std::string serial_port;
43 
44  vpRobotAfma6 *robot;
45  geometry_msgs::PoseStamped position;
46 
47  //for odom->base_link transform
49  geometry_msgs::TransformStamped odom_trans;
50  //for resolving tf names.
51  std::string tf_prefix;
52  std::string frame_id_odom;
53  std::string frame_id_base_link;
54 
55  vpHomogeneousMatrix wMc; // world to camera transformation
56  vpColVector q; // measured joint position
57  };
58 
59 
61 {
62  // read in config options
63  n = nh;
64 
65  ROS_INFO( "using Afma6 robot" );
66 
67  robot = NULL;
68  /*
69  * Figure out what frame_id's to use. if a tf_prefix param is specified,
70  * it will be added to the beginning of the frame_ids.
71  *
72  * e.g. rosrun ... _tf_prefix:=MyRobot (or equivalently using <param>s in
73  * roslaunch files)
74  * will result in the frame_ids being set to /MyRobot/odom etc,
75  * rather than /odom. This is useful for Multi Robot Systems.
76  * See ROS Wiki for further details.
77  */
79 // frame_id_odom = tf::resolve(tf_prefix, "odom");
80 // frame_id_base_link = tf::resolve(tf_prefix, "base_link");
81 
82  // advertise services
83  pose_pub = n.advertise<geometry_msgs::PoseStamped>("pose", 1000);
84  vel_pub = n.advertise<geometry_msgs::TwistStamped>("velocity", 1000);
85 
86  // subscribe to services
87  cmd_camvel_sub = n.subscribe( "cmd_camvel", 1, (boost::function < void(const geometry_msgs::TwistStampedConstPtr&)>) boost::bind( &RosAfma6Node::setCameraVel, this, _1 ));
88 }
89 
91 {
92  if (robot) {
93  robot->stopMotion();
94  delete robot;
95  robot = NULL;
96  }
97 }
98 
100 {
101  robot = new vpRobotAfma6;
102 
103  robot->init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithDistortion);
104  vpCameraParameters cam;
105  robot->getCameraParameters(cam, 640, 480);
106  std::cout << "Camera parameters (640 x 480):\n" << cam << std::endl;
107 
108  robot->setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
109 
110  return 0;
111 }
112 
114 {
115  ros::Rate loop_rate(100);
116  while(ros::ok()){
117  this->publish();
118  ros::spinOnce();
119  loop_rate.sleep();
120  }
121 // ros::spin();
122 }
123 
125 {
126  double timestamp;
127  robot->getPosition(vpRobot::ARTICULAR_FRAME, q, timestamp);
128  wMc = robot->get_fMc(q);
130  position.header.stamp = ros::Time(timestamp); // to improve: should be the timestamp returned by getPosition()
131 
132  // ROS_INFO( "Afma6 publish pose at %f s: [%0.2f %0.2f %0.2f] - [%0.2f %0.2f %0.2f %0.2f]",
133  // position.header.stamp.toSec(),
134  // position.pose.position.x, position.pose.position.y, position.pose.position.z,
135  // position.pose.orientation.w, position.pose.orientation.x, position.pose.orientation.y, position.pose.orientation.z);
137 
138  vpColVector vel(6);
139  robot->getVelocity(vpRobot::CAMERA_FRAME, vel, timestamp);
140  geometry_msgs::TwistStamped vel_msg;
141  vel_msg.header.stamp = ros::Time(timestamp);
142  vel_msg.twist.linear.x = vel[0];
143  vel_msg.twist.linear.y = vel[1];
144  vel_msg.twist.linear.z = vel[2];
145  vel_msg.twist.angular.x = vel[3];
146  vel_msg.twist.angular.y = vel[4];
147  vel_msg.twist.angular.z = vel[5];
148  vel_pub.publish(vel_msg);
149 
150 // ros::Duration(1e-3).sleep();
151 }
152 
153 void
154 RosAfma6Node::setCameraVel( const geometry_msgs::TwistStampedConstPtr &msg)
155 {
157 
158  vpColVector vc(6); // Vel in m/s and rad/s
159 
160  vc[0] = msg->twist.linear.x;
161  vc[1] = msg->twist.linear.y;
162  vc[2] = msg->twist.linear.z;
163 
164  vc[3] = msg->twist.angular.x;
165  vc[4] = msg->twist.angular.y;
166  vc[5] = msg->twist.angular.z;
167 
168 // 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",
169 // veltime.toSec(),
170 // vc[0], vc[1], vc[2], vc[3], vc[4], vc[5]);
171  robot->setVelocity(vpRobot::CAMERA_FRAME, vc);
172 
173 // this->publish();
174 }
175 
176 #endif // #ifdef VISP_HAVE_AFMA6
177 
178 int main( int argc, char** argv )
179 {
180 #ifdef VISP_HAVE_AFMA6
181  ros::init(argc,argv, "RosAfma6");
182  ros::NodeHandle n(std::string("~"));
183 
184  RosAfma6Node *node = new RosAfma6Node(n);
185 
186  if( node->setup() != 0 )
187  {
188  printf( "Afma6 setup failed... \n" );
189  return -1;
190  }
191 
192  node->spin();
193 
194  delete node;
195 
196  printf( "\nQuitting... \n" );
197 #else
198  printf("This node is node available since ViSP was \nnot build with Afma6 robot support...\n");
199 #endif
200  return 0;
201 }
202 
RosAfma6Node(ros::NodeHandle n)
Definition: afma6.cpp:60
void spin()
Definition: afma6.cpp:113
void publish()
Definition: afma6.cpp:124
vpColVector q
Definition: afma6.cpp:56
std::string frame_id_odom
Definition: afma6.cpp:52
void publish(const boost::shared_ptr< M > &message) const
std::string getPrefixParam(ros::NodeHandle &nh)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
std::string frame_id_base_link
Definition: afma6.cpp:53
vpRobotAfma6 * robot
Definition: afma6.cpp:44
geometry_msgs::PoseStamped position
Definition: afma6.cpp:45
void setCameraVel(const geometry_msgs::TwistStampedConstPtr &)
Definition: afma6.cpp:154
int main(int argc, char **argv)
Definition: afma6.cpp:178
ros::Time veltime
Definition: afma6.cpp:40
geometry_msgs::Pose toGeometryMsgsPose(const vpHomogeneousMatrix &mat)
ros::NodeHandle n
Definition: afma6.cpp:35
int setup()
Definition: afma6.cpp:99
ros::Publisher vel_pub
Definition: afma6.cpp:37
std::string tf_prefix
Definition: afma6.cpp:51
#define ROS_INFO(...)
virtual ~RosAfma6Node()
Definition: afma6.cpp:90
ROSCPP_DECL bool ok()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool sleep()
vpHomogeneousMatrix wMc
Definition: afma6.cpp:55
static Time now()
ros::Subscriber cmd_camvel_sub
Definition: afma6.cpp:38
geometry_msgs::TransformStamped odom_trans
Definition: afma6.cpp:49
ros::Publisher pose_pub
Definition: afma6.cpp:36
ROSCPP_DECL void spinOnce()
tf::TransformBroadcaster odom_broadcaster
Definition: afma6.cpp:48
std::string serial_port
Definition: afma6.cpp:42


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