相比于笨拙的全局变量和全局函数,将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);
}