In the previous article, the algorithm was sloppy:
ROS robot from the starting point to the end point (1) Simple P control
Can it be improved a bit? Refer to the following code:
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);
Compared:
vel_x=(goal_x-msg->x)/8.0;
vel_z=(goal_y-msg->y)/40.0;
Revise:
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;
}
How does it work? ? ?
Shocked, are you? ? ?
The 10.0 and 1.5 targets are indeed there, but?
The final accuracy is quite high! ! !
Can this magical move be tolerated?
You can change the parameters.