目的:为了实现一种简单的跟随
#include "ros/ros.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/PoseStamped.h"
/**
* This tutorial demonstrates simple receipt of messages over the ROS system.
*/
class mySubAndPub
{
public:
mySubAndPub();
void poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg);
private:
//geometry_msgs::PoseWithCovarianceStamped::ConstPtr msg_global;
ros::NodeHandle handle;
ros::Subscriber sub;
ros::Publisher goal_pub;
};
mySubAndPub::mySubAndPub()
{
//msg_global=NULL;
sub = handle.subscribe("/robot1/amcl_pose", 1, &mySubAndPub::poseCallback,this);
goal_pub = handle.advertise<geometry_msgs::PoseStamped>("/robot2/move_base_simple/goal", 1);
}
void mySubAndPub::poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg)
{
ROS_INFO("Robot1 position(%lf,%lf)", msg->pose.pose.position.x, msg->pose.pose.position.y);
geometry_msgs::PoseStamped goal_msg;
goal_msg.header.frame_id = "map";
goal_msg.pose.orientation = msg->pose.pose.orientation;
goal_msg.pose.position = msg->pose.pose.position;
//goal_msg.pose.position.x = msg_global->pose.pose.position.x;
goal_pub.publish(goal_msg);
ROS_INFO("Goal published");
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "mySubAndPub");
mySubAndPub sap;
ros::spin();
return 0;
}
版权声明:本文为YangZuo_Chester原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。