5 #include <geometry_msgs/Pose.h> 6 #include <geometry_msgs/PoseStamped.h> 7 #include <geometry_msgs/TwistStamped.h> 8 #include <nav_msgs/Odometry.h> 15 #include <visp/vpRobotAfma6.h> 19 #ifdef VISP_HAVE_AFMA6 29 void setCameraVel(
const geometry_msgs::TwistStampedConstPtr & );
88 (boost::function<
void(
const geometry_msgs::TwistStampedConstPtr & ) >)boost::bind(
107 m_robot->init( static_cast< vpAfma6::vpAfma6ToolType >(
m_tool_type ), vpCameraParameters::perspectiveProjWithDistortion );
108 vpCameraParameters cam;
109 m_robot->getCameraParameters( cam, 640, 480 );
110 std::cout <<
"Camera parameters (640 x 480):\n" << cam << std::endl;
112 m_robot->setRobotState( vpRobot::STATE_VELOCITY_CONTROL );
134 m_robot->getPosition( vpRobot::ARTICULAR_FRAME,
m_q, timestamp );
146 vpColVector vel( 6 );
147 m_robot->getVelocity( vpRobot::CAMERA_FRAME, vel, timestamp );
148 geometry_msgs::TwistStamped vel_msg;
149 vel_msg.header.stamp =
ros::Time( timestamp );
150 vel_msg.twist.linear.x = vel[0];
151 vel_msg.twist.linear.y = vel[1];
152 vel_msg.twist.linear.z = vel[2];
153 vel_msg.twist.angular.x = vel[3];
154 vel_msg.twist.angular.y = vel[4];
155 vel_msg.twist.angular.z = vel[5];
168 vc[0] = msg->twist.linear.x;
169 vc[1] = msg->twist.linear.y;
170 vc[2] = msg->twist.linear.z;
172 vc[3] = msg->twist.angular.x;
173 vc[4] = msg->twist.angular.y;
174 vc[5] = msg->twist.angular.z;
179 m_robot->setVelocity( vpRobot::CAMERA_FRAME, vc );
184 #endif // #ifdef VISP_HAVE_AFMA6 189 #ifdef VISP_HAVE_AFMA6 195 if ( node.
setup() != 0 )
197 printf(
"Afma6 setup failed... \n" );
203 catch(
const vpException &e)
205 std::cout <<
"Catch exception: " << e.getMessage() << std::endl;
208 printf(
"\nQuitting... \n" );
210 printf(
"This node is node available since ViSP was \nnot build with Afma6 robot support...\n" );
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())
vpHomogeneousMatrix m_wMc
std::string m_serial_port
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void setCameraVel(const geometry_msgs::TwistStampedConstPtr &)
int main(int argc, char **argv)
geometry_msgs::Pose toGeometryMsgsPose(const vpHomogeneousMatrix &mat)
void publish(const boost::shared_ptr< M > &message) const
bool getParam(const std::string &key, std::string &s) const
tf::TransformBroadcaster m_odom_broadcaster
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
geometry_msgs::TransformStamped m_odom_trans
ros::Publisher m_pose_pub
ros::Subscriber m_cmd_camvel_sub
std::string m_frame_id_odom
std::string m_frame_id_base_link
geometry_msgs::PoseStamped m_position
ROSCPP_DECL void spinOnce()