6 #include <geometry_msgs/TwistStamped.h>
7 #include <geometry_msgs/Pose.h>
8 #include <geometry_msgs/PoseStamped.h>
9 #include <nav_msgs/Odometry.h>
11 #include <tf/transform_listener.h>
12 #include <tf/transform_broadcaster.h>
13 #include <tf/transform_datatypes.h>
15 #include <visp/vpRobotAfma6.h>
16 #include <visp/vpRingLight.h>
18 #include <visp_bridge/3dpose.h>
21 #ifdef VISP_HAVE_AFMA6
31 void setCameraVel(
const geometry_msgs::TwistStampedConstPtr &);
56 vpHomogeneousMatrix
wMc;
66 ROS_INFO(
"using Afma6 robot" );
84 pose_pub =
n.advertise<geometry_msgs::PoseStamped>(
"pose", 1000);
85 vel_pub =
n.advertise<geometry_msgs::TwistStamped>(
"velocity", 1000);
102 robot =
new vpRobotAfma6;
104 robot->init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithDistortion);
106 robot->setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
113 ros::Rate loop_rate(100);
125 robot->getPosition(vpRobot::ARTICULAR_FRAME,
q, timestamp);
127 position.pose = visp_bridge::toGeometryMsgsPose(
wMc);
128 position.header.stamp = ros::Time(timestamp);
137 robot->getVelocity(vpRobot::CAMERA_FRAME, vel, timestamp);
138 geometry_msgs::TwistStamped vel_msg;
139 vel_msg.header.stamp = ros::Time(timestamp);
140 vel_msg.twist.linear.x = vel[0];
141 vel_msg.twist.linear.y = vel[1];
142 vel_msg.twist.linear.z = vel[2];
143 vel_msg.twist.angular.x = vel[3];
144 vel_msg.twist.angular.y = vel[4];
145 vel_msg.twist.angular.z = vel[5];
158 vc[0] = msg->twist.linear.x;
159 vc[1] = msg->twist.linear.y;
160 vc[2] = msg->twist.linear.z;
162 vc[3] = msg->twist.angular.x;
163 vc[4] = msg->twist.angular.y;
164 vc[5] = msg->twist.angular.z;
169 robot->setVelocity(vpRobot::CAMERA_FRAME, vc);
174 #endif // #ifdef VISP_HAVE_AFMA6
176 int main(
int argc,
char** argv )
178 #ifdef VISP_HAVE_AFMA6
179 ros::init(argc,argv,
"RosAfma6");
180 ros::NodeHandle n(std::string(
"~"));
187 if( node->
setup() != 0 )
189 printf(
"Afma6 setup failed... \n" );
199 printf(
"\nQuitting... \n" );
201 printf(
"This node is node available since ViSP was \nnot build with Afma6 robot support...\n");
RosAfma6Node(ros::NodeHandle n)
std::string frame_id_odom
std::string frame_id_base_link
geometry_msgs::PoseStamped position
void setCameraVel(const geometry_msgs::TwistStampedConstPtr &)
int main(int argc, char **argv)
ros::Subscriber cmd_camvel_sub
geometry_msgs::TransformStamped odom_trans
tf::TransformBroadcaster odom_broadcaster