ROS机器人从起点到终点（二）改进

最新推荐文章于 2023-11-21 04:50:18 发布

原创最新推荐文章于 2023-11-21 04:50:18 发布 · 1.2k 阅读

2 ·

CC 4.0 BY-SA版权

文章标签：

#蓝桥ROS #turtlesim

博客介绍了如何改进ROS机器人的P控制算法，通过调整计算方式，实现了从起点到终点的路径规划。代码示例展示了如何计算线性和角速度，以达到更高的精度。虽然路径走位显得有些奇特，但最终的定位精度得到了显著提高。

之前一篇，算法稀烂：

ROS机器人从起点到终点（一）简单P控制

能否改进一点呢？参考如下代码：

		geometry_msgs::Twist vel_msg;
		vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
				                        transform.getOrigin().x());
		vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
				                      pow(transform.getOrigin().y(), 2));
		turtle_vel.publish(vel_msg);

对比：


    vel_x=(goal_x-msg->x)/8.0;
    vel_z=(goal_y-msg->y)/40.0;

修改：

vel_x = 1.0 * sqrt(pow((goal_x-msg->x), 2) + pow((goal_y-msg->y), 2));

vel_z = 4.0 * atan2((goal_y-msg->y), (goal_x-msg->x));

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include "turtlesim/Pose.h"
 
float goal_x=10.0,goal_y=1.5,vel_x=0,vel_z=0;
 
void poseCallback(const turtlesim::Pose::ConstPtr& msg)
{
    ROS_INFO("Turtle pose: x:%0.6f, y:%0.6f", msg->x, msg->y);

    vel_x = 1.0 * sqrt(pow((goal_x-msg->x), 2) + pow((goal_y-msg->y), 2));

    vel_z = 4.0 * atan2((goal_y-msg->y), (goal_x-msg->x));
}
 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "pose_sub_vel_pub");
 
    ros::NodeHandle n;
 
    ros::Subscriber pose_sub = n.subscribe("/turtle1/pose", 10, poseCallback);
 
    ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
    
    ros::Rate loop_rate(10);
 
    int count = 0;
    geometry_msgs::Twist vel_msg;
    vel_msg.linear.x = 0.0;
    vel_msg.angular.z = 0.0;
 
 
    while (ros::ok())
    {
	vel_msg.linear.x=vel_x;
	vel_msg.angular.z=vel_z;
	turtle_vel_pub.publish(vel_msg);
	ROS_INFO("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]", 
				vel_msg.linear.x, vel_msg.angular.z);
        ros::spinOnce();	
	loop_rate.sleep();
    }
 
    return 0;
}

试一试效果如何？？？