biclops.cpp
Go to the documentation of this file.
1 #include <stdio.h>
2 #include <math.h>
3 #include <visp/vpRobotBiclops.h> // visp
4 
5 #include <visp_bridge/3dpose.h> // visp_bridge
6 
7 #include <ros/ros.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 
18 #include <sstream>
19 
20 #ifdef VISP_HAVE_BICLOPS
21 
23 {
24  public:
26  virtual ~RosBiclopsNode();
27 
28  public:
29  int setup();
30  void setJointVel( const geometry_msgs::TwistConstPtr &);
31  void setJointPos( const geometry_msgs::PoseConstPtr &);
32  void spin();
33  void publish();
34 
35  protected:
41 
43 
44  std::string serial_port;
45 
46  vpRobotBiclops *robot;
47  geometry_msgs::PoseStamped position;
48 
49  //for odom->base_link transform
51  geometry_msgs::TransformStamped odom_trans;
52  //for resolving tf names.
53  std::string tf_prefix;
54  std::string frame_id_odom;
55  std::string frame_id_base_link;
56 
57  vpHomogeneousMatrix wMc; // world to camera transformation
58  vpColVector q; // measured joint position
59 
60 };
61 
62 
64 {
65  // read in config options
66  n = nh;
67 
68  ROS_INFO( "Using Biclops robot" );
69 
70  robot = NULL;
71  /*
72  * Figure out what frame_id's to use. if a tf_prefix param is specified,
73  * it will be added to the beginning of the frame_ids.
74  *
75  * e.g. rosrun ... _tf_prefix:=MyRobot (or equivalently using <param>s in
76  * roslaunch files)
77  * will result in the frame_ids being set to /MyRobot/odom etc,
78  * rather than /odom. This is useful for Multi Robot Systems.
79  * See ROS Wiki for further details.
80  */
82  // frame_id_odom = tf::resolve(tf_prefix, "odom");
83  // frame_id_base_link = tf::resolve(tf_prefix, "base_link");
84 
85  // advertise services
86  pose_pub = n.advertise<geometry_msgs::PoseStamped>("biclops/odom", 1000);
87 
88  // subscribe to services
89  cmd_jointvel_sub = n.subscribe( "cmd_vel", 1, (boost::function < void(const geometry_msgs::TwistConstPtr&)>) boost::bind( &RosBiclopsNode::setJointVel, this, _1 ));
90  cmd_jointpos_sub = n.subscribe( "pose", 1, (boost::function < void(const geometry_msgs::PoseConstPtr&)>) boost::bind( &RosBiclopsNode::setJointPos, this, _1 ));
91 }
92 
94 {
95  if (robot) {
96  robot->stopMotion();
97  delete robot;
98  robot = NULL;
99  }
100 }
101 
103 {
104  robot = new vpRobotBiclops("/usr/share/BiclopsDefault.cfg");
105  robot->setDenavitHartenbergModel(vpBiclops::DH2);
106 
107  vpColVector qinit(2);
108  qinit = 0;
109  robot->setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
110  robot->setPositioningVelocity(40);
111  robot->setPosition(vpRobot::ARTICULAR_FRAME, qinit);
112 
113  robot->setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
114 
115  return 0;
116 }
117 
119 {
120  ros::Rate loop_rate(15);
121  while(ros::ok()){
122  this->publish();
123  ros::spinOnce();
124  loop_rate.sleep();
125  }
126  // ros::spin();
127 }
128 
130 {
131  robot->getPosition(vpRobot::ARTICULAR_FRAME, q);
132 
133  position.pose.position.x = 0;
134  position.pose.position.y = 0;
135  position.pose.position.z = 0;
136  position.pose.orientation.x = q[1];
137  position.pose.orientation.y = q[0];
138  position.pose.orientation.z = 0;
139  position.pose.orientation.w = 0;
140 
141  position.header.stamp = ros::Time::now();
142 
143  // ROS_INFO( "Biclops publish pose at %f s: [%0.2f %0.2f %0.2f] - [%0.2f %0.2f %0.2f %0.2f]",
144  // position.header.stamp.toSec(),
145  // position.pose.position.x, position.pose.position.y, position.pose.position.z,
146  // position.pose.orientation.w, position.pose.orientation.x, position.pose.orientation.y, position.pose.orientation.z);
148 }
149 
150 void
151 RosBiclopsNode::setJointVel( const geometry_msgs::TwistConstPtr &msg)
152 {
154 
155  vpColVector qdot(2); // Vel in rad/s for pan and tilt
156 
157  qdot[1] = msg->angular.x;
158  qdot[0] = msg->angular.y;
159 
160 
161  ROS_INFO( "Biclops new joint vel at %f s: [%0.2f %0.2f] rad/s",
162  veltime.toSec(),
163  qdot[0], qdot[1]);
164  robot->setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
165  robot->setVelocity(vpRobot::ARTICULAR_FRAME, qdot);
166 }
167 void
168 RosBiclopsNode::setJointPos( const geometry_msgs::PoseConstPtr &msg)
169 {
171 
172  vpColVector qdes(2); // Vel in rad/s for pan and tilt
173 
174  qdes[0] = msg->orientation.x;
175  qdes[1] = msg->orientation.y;
176 
177  ROS_INFO( "Biclops new joint pos at %f s: [%0.2f %0.2f] rad/s",
178  veltime.toSec(),
179  qdes[0], qdes[1]);
180  robot->setRobotState(vpRobot::STATE_POSITION_CONTROL);
181  robot->setPosition(vpRobot::ARTICULAR_FRAME, qdes);
182 }
183 
184 #endif // #ifdef VISP_HAVE_BICLOPS
185 
186 
187 int main( int argc, char** argv )
188 {
189 #ifdef VISP_HAVE_BICLOPS
190  ros::init(argc,argv, "RosBiclops");
191  ros::NodeHandle n(std::string("~"));
192 
193  RosBiclopsNode *node = new RosBiclopsNode(n);
194 
195  if( node->setup() != 0 )
196  {
197  printf( "Biclops setup failed... \n" );
198  return -1;
199  }
200 
201  node->spin();
202 
203  delete node;
204 
205  printf( "\nQuitting... \n" );
206 #else
207  printf("This node is node available since ViSP was \nnot build with Biclops robot support...\n");
208 #endif
209  return 0;
210 }
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())
std::string tf_prefix
Definition: biclops.cpp:53
geometry_msgs::TransformStamped odom_trans
Definition: biclops.cpp:51
geometry_msgs::PoseStamped position
Definition: biclops.cpp:47
std::string frame_id_base_link
Definition: biclops.cpp:55
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void setJointVel(const geometry_msgs::TwistConstPtr &)
Definition: biclops.cpp:151
virtual ~RosBiclopsNode()
Definition: biclops.cpp:93
ros::Publisher pose_pub
Definition: biclops.cpp:37
std::string serial_port
Definition: biclops.cpp:44
RosBiclopsNode(ros::NodeHandle n)
Definition: biclops.cpp:63
ros::Subscriber cmd_jointpos_sub
Definition: biclops.cpp:40
#define ROS_INFO(...)
ros::Time veltime
Definition: biclops.cpp:42
ros::NodeHandle n
Definition: biclops.cpp:36
ROSCPP_DECL bool ok()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
tf::TransformBroadcaster odom_broadcaster
Definition: biclops.cpp:50
void publish()
Definition: biclops.cpp:129
bool sleep()
std::string frame_id_odom
Definition: biclops.cpp:54
ros::Subscriber cmd_jointvel_sub
Definition: biclops.cpp:39
vpRobotBiclops * robot
Definition: biclops.cpp:46
static Time now()
ROSCPP_DECL void spinOnce()
vpHomogeneousMatrix wMc
Definition: biclops.cpp:57
int main(int argc, char **argv)
Definition: biclops.cpp:187
void setJointPos(const geometry_msgs::PoseConstPtr &)
Definition: biclops.cpp:168
vpColVector q
Definition: biclops.cpp:58
ros::Publisher vel_pub
Definition: biclops.cpp:38


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