目的:为了实现一种简单的跟随

#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 版权协议,转载请附上原文出处链接和本声明。
原文链接:https://blog.csdn.net/YangZuo_Chester/article/details/109388388