ROS机器人从起点到终点完成

Posted 2022-04-23 zhangrelay

这是一个起点到终点勉强及格的完成版本。

输入坐标，到达指定位置！

最终效果如下：

 

修改CMakelist：

add_executable(move src/move.cpp)
target_link_libraries(move $catkin_LIBRARIES)
add_dependencies(move turtlesim_gencpp)

---

程序参考：

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "geometry_msgs/Twist.h"
#include "math.h"

#include <sstream>

ros::Subscriber sub;
ros::Publisher pub;
float goal_x = 2;
float goal_y = 2;

void sendVel(const turtlesim::Pose::ConstPtr& data)
    ros::NodeHandle n;

    pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",100);

    float curr_x = data->x;
    float curr_y = data->y;
    float curr_ang = data->theta;

    float dist = sqrt(pow(goal_x-curr_x,2) + pow(goal_y-curr_y,2));
    std::cout << "Distance = " << dist << std::endl;

    if(dist > 0.01)
        double ang = atan2((float)(goal_y-curr_y),(float)(goal_x-curr_x));

        std::cout << "Curr_ang = " << curr_ang << " | ang = " << ang << std::endl;

        geometry_msgs::Twist t_msg;

        t_msg.linear.x = 1.0*(dist);
        t_msg.angular.z = 4.0*(ang-curr_ang);

        pub.publish(t_msg);
    
    else
    
    	std::cout << "Mission Completed" << std::endl;
    	std::cout << "Please enter new coordinates" << std::endl;
    	std::cout << "Please enter goal_x:" << std::endl;
    	std::cin >> goal_x;
    	std::cout << "Please enter goal_y:" << std::endl;
    	std::cin >> goal_y;
    



int main(int argc, char **argv)
    ros::init(argc,argv,"goToGoal");

    ros::NodeHandle n;

    sub = n.subscribe("/turtle1/pose",100,sendVel);

    ros::spin();


    return 0;

---

运行命令如下：

rosrun turtlesim move
 

---

过程中的一些图：

 

 

 

 

---