将ros中suscriber和publisher写入class中
相比于笨拙的全局变量和全局函数,将suscriber和publisher成一个class,形式更加简洁和容易管理,一个节点就是一个类
参考资料
http://answers.ros.org/question/59725/publishing-to-a-topic-via-subscriber-callback-function/
http://wiki.ros.org/roscpp_tutorials/Tutorials/UsingClassMethodsAsCallbacks
下面是自己写的示例代码:
#include <ros/ros.h> #include <tf/transform_broadcaster.h> #include <nav_msgs/Odometry.h> #include <geometry_msgs/Pose2D.h> #include <tf/transform_listener.h> #include <stdio.h> class test { private: ros::NodeHandle nh_; ros::Subscriber sub_; tf::TransformBroadcaster br_; bool data_ready; public: test(ros::NodeHandle& nh) { nh_=nh; sub_ = nh_.subscribe("/user_set", 10, &test::call_back,this); data_ready = false; }
//注意这里需要加const void call_back(const geometry_msgs::Pose2DPtr& msgs) { ROS_INFO("recive"); tf::Transform dest_transform; dest_transform.setOrigin(tf::Vector3(msgs->x,msgs->y,0)); tf::Quaternion q; q.setRPY(0, 0, msgs->theta); dest_transform.setRotation(q); br_.sendTransform(tf::StampedTransform(dest_transform,ros::Time::now(),"world","user_set_frame")); data_ready = true; } bool is_data_ready() { if(data_ready) return true; else return false; } }; int main(int argc, char** argv)
{ ros::init(argc, argv, "tf_broadcaster"); ros::NodeHandle node; test Otest(node); tf::TransformListener listener; tf::StampedTransform transform; while(ros::ok()) { if(!Otest.is_data_ready()) { ros::spinOnce(); continue; } ROS_INFO("lookup_transfoem;"); try { listener.waitForTransform("world","user_set_frame",ros::Time::now(), ros::Duration(1.0)); listener.lookupTransform("world","user_set_frame",ros::Time(0),transform); } catch (tf::TransformException ex) { ROS_ERROR("%s",ex.what()); break; } tf::Vector3 vectortf = transform.getOrigin(); ROS_INFO("transform.x:%f,transform.y:%f,transform.z:%f",vectortf.x(),vectortf.y(),vectortf.z()); ros::spinOnce(); } return 0; }
下面是publisher,上面那段代码调好了,下面还没有
#include <iostream> #include <ros/ros.h> #include <geometry_msgs/Twist.h> #include <geometry_msgs/Pose2D.h> #include <tf/transform_listener.h> #include <tf/transform_broadcaster.h> class RobotDriver { private: //! The node handle we'll be using ros::NodeHandle nh_; //! We will be publishing to the "cmd_vel" topic to issue commands ros::Publisher cmd_vel_pub_; //! we will be suscribing the topic "pos_dest" ros::Subscriber sub = n.subscribe("pos_dest", 10, pos_dest_callback); //! pos_dest_callback tf::Transform dest_transform; void pos_dest_callback(geometry_msgs::Pose2DPtr& msgs) { static tf::TransformBroadcaster br; dest_transform.setOrigin(tf::Vector3(msgs->x,msgs->y,0)); tf::Quaternion q; q.setRPY(0, 0, msgs->theta); dest_transform.setRotation(q); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "odom", "pos_dest")); } //! We will be listening to TF transforms as well tf::TransformListener listener_; //! cmd geometry_msgs::Twist base_cmd_straight; geometry_msgs::Twist base_cmd_turn; geometry_msgs::Twist stop_cmd; tf::Transform current_transform; public: //! ROS node initialization RobotDriver(ros::NodeHandle &nh) { nh_ = nh; //set up the publisher for the cmd_vel topic cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1); //cmd //the command will be to go forward at 0.25 m/s base_cmd_straight.linear.y = base_cmd_straight.angular.z = 0; base_cmd.base_cmd_straight.x = 0.1; base_cmd_turn.linear.x = base_cmd_turn.linear.y = 0.0; base_cmd_turn.angular.z = 0.75; stop_cmd.linear.y = stop_cmd.angular.z = 0; stop_cmd.linear.x = 0; } bool turnOdom(bool clockwise, double radians) { while(radians < 0) radians += 2*M_PI; while(radians > 2*M_PI) radians -= 2*M_PI; //wait for the listener to get the first message listener_.waitForTransform("base_link", "odom", ros::Time(0), ros::Duration(1.0)); //we will record transforms here tf::StampedTransform start_transform; tf::StampedTransform current_transform; //record the starting transform from the odometry to the base frame listener_.lookupTransform("base_link", "odom", ros::Time(0), start_transform); //we will be sending commands of type "twist" geometry_msgs::Twist base_cmd; //the command will be to turn at 0.75 rad/s base_cmd.linear.x = base_cmd.linear.y = 0.0; base_cmd.angular.z = 0.75; geometry_msgs::Twist stop_cmd; stop_cmd.linear.y = stop_cmd.angular.z = 0; stop_cmd.linear.x = 0; if (clockwise) base_cmd.angular.z = -base_cmd.angular.z; //the axis we want to be rotating by tf::Vector3 desired_turn_axis(0,0,1); if (!clockwise) desired_turn_axis = -desired_turn_axis; ros::Rate rate(10.0); bool done = false; while (!done && nh_.ok()) { //send the drive command cmd_vel_pub_.publish(base_cmd); rate.sleep(); //get the current transform try { listener_.lookupTransform("base_link", "odom", ros::Time(0), current_transform); } catch (tf::TransformException ex) { ROS_ERROR("%s",ex.what()); break; } tf::Transform global_transform = current_transform; tf::Vector3 actual_turn_axis = global_transform.getRotation().getAxis(); ROS_INFO("actual_turn_axis.x=%f,actual_turn_axis.y=%f,actual_turn_axis.z=%f",(float)actual_turn_axis.getX(),(float)actual_turn_axis.getY(),(float)actual_turn_axis.getZ()); double angle_turned = global_transform.getRotation().getAngle(); if ( fabs(angle_turned) < 1.0e-2) continue; if ( actual_turn_axis.dot( desired_turn_axis ) < 0 ) angle_turned = 2 * M_PI - angle_turned; if (angle_turned > radians) done = true; } if (done) { cmd_vel_pub_.publish(stop_cmd); return true; } return false; } }; int main(int argc, char** argv) { //init the ROS node ros::init(argc, argv, "robot_driver"); ros::NodeHandle nh; RobotDriver driver(nh); driver.driveForwardOdom(0.2); }