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

Posted 2022-04-13 zhangrelay

这部分可以参考之前案例：

☞ ROS编程（ETH）2018更新版习题说明（三）_zhangrelay的博客-CSDN博客

---

遥控肯定没难度，但是精度也很差哦。

 

---

获取目标位置与当前位置之间的误差，用于机器人运动控制。

假定目标x=10.0 y=1.5，求解机器人控制速度？ 

获取位置：

#include <ros/ros.h>
#include "turtlesim/Pose.h"

void poseCallback(const turtlesim::Pose::ConstPtr& msg)

    ROS_INFO("Turtle pose: x:%0.6f, y:%0.6f", msg->x, msg->y);


int main(int argc, char **argv)

    ros::init(argc, argv, "pose_subscriber");

    ros::NodeHandle n;

    ros::Subscriber pose_sub = n.subscribe("/turtle1/pose", 10, poseCallback);

    ros::spin();

    return 0;

发布速度：

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

int main(int argc, char **argv)

	ros::init(argc, argv, "velocity_publisher");

	ros::NodeHandle n;

	ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);

	ros::Rate loop_rate(10);

	int count = 0;
	while (ros::ok())
	
		geometry_msgs::Twist vel_msg;
		vel_msg.linear.x = 0.5;
		vel_msg.angular.z = 0.2;

		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);

	    loop_rate.sleep();
	

	return 0;

暴力揉到一起…………………………不推荐……………………

 

 

 

#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=(goal_x-msg->x)/8.0;
    vel_z=(goal_y-msg->y)/40.0;


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;

更好用的代码需要自己研究哦 。

---