ROS robot from start point to end point (1) Simple P control

This part can refer to the previous case:

☞  ROS Programming (ETH) 2018 Update Exercise Instructions (3)_zhangrelay's Blog - CSDN Blog

---

The remote control is certainly not difficult, but the accuracy is also very poor.

 

---

Get the error between the target position and the current position for robot motion control.

Assuming the target x=10.0 y=1.5, solve the robot control speed? 

Get location:

#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;
}

Release speed:

#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;
}

Violent rubbing together…………………………Not recommended………………

 

 

 

#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;
}

Better code needs to be researched by yourself.

---