ROS机器人从起点到终点（三）完成

最新推荐文章于 2022-11-02 21:23:50 发布

原创最新推荐文章于 2022-11-02 21:23:50 发布 · 1.3k 阅读

4 ·

CC 4.0 BY-SA版权

文章标签：

#turtlesim

该博客介绍了如何使用ROS（Robot Operating System）和Turtlebot进行机器人定位与导航。通过修改CMakeLists文件和编写程序，实现了从起点到终点的路径规划。当机器人距离目标小于0.01时，用户可以输入新的目标坐标。程序通过订阅/turtle1/pose话题获取当前位姿，并发布/turtle1/cmd_vel消息来调整速度，使机器人朝目标移动。

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

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

最终效果如下：

修改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

过程中的一些图：