Robot Motion in ROS
In this tutorial, the movement of the turtlebot can be done with respect to the real-time robot simulation. The modification of the changes in the previous tutorial that is the simulation of the turtlebot can be done to run the real-time robot accordingly. robot motion in ROS
To install the simulator run the following command
sudo apt-get install ros-kinetic-turtlebot-*Now, you can run the simulation of the robot by running the following command
roslaunch turtlebot_stage turtlebot_in_stage.launch
You can run these two following commands to set the environment and re launch the rviz simulator
export TURTLEBOT_STAGE_WORLD_FILE="~/stageTutorial/maps/stage/maze.world"
export TURTLEBOT_STAGE_MAP_FILE="~/stageTutorial/maps/maze.yaml"
Code
Now run the following code to execute the working of the turtle bot simulation with a robot
import rospy
from geometry_msgs.msg import Twist
import math
import time
from std_srvs.srv import Empty
def move(speed, distance, is_forward):
#declare a Twist message to send velocity commands
velocity_message = Twist()
#get current location
if (speed > 0.4):
print 'speed must be lower than 0.4'
return
if (is_forward):
velocity_message.linear.x =abs(speed)
else:
velocity_message.linear.x =-abs(speed)
distance_moved = 0.0
loop_rate = rospy.Rate(10) # we publish the velocity at 10 Hz (10 times a second)
cmd_vel_topic='/cmd_vel_mux/input/teleop'
velocity_publisher = rospy.Publisher(cmd_vel_topic, Twist, queue_size=10)
t0 = rospy.Time.now().to_sec()
while True :
rospy.loginfo("Turtlesim moves forwards")
velocity_publisher.publish(velocity_message)
loop_rate.sleep()
t1 = rospy.Time.now().to_sec()
#rospy.Duration(1.0)
distance_moved = (t1-t0) * speed
print distance_moved
if not (distance_moved<distance):
rospy.loginfo("reached")
break
#finally, stop the robot when the distance is moved
velocity_message.linear.x =0
velocity_publisher.publish(velocity_message)
def rotate (angular_speed_degree, relative_angle_degree, clockwise):
velocity_message = Twist()
velocity_message.linear.x=0
velocity_message.linear.y=0
velocity_message.linear.z=0
velocity_message.angular.x=0
velocity_message.angular.y=0
velocity_message.angular.z=0
angular_speed=math.radians(abs(angular_speed_degree))
if (clockwise):
velocity_message.angular.z =-abs(angular_speed)
else:
velocity_message.angular.z =abs(angular_speed)
angle_moved = 0.0
loop_rate = rospy.Rate(10) # we publish the velocity at 10 Hz (10 times a second)
cmd_vel_topic='/cmd_vel_mux/input/teleop'
velocity_publisher = rospy.Publisher(cmd_vel_topic, Twist, queue_size=10)
t0 = rospy.Time.now().to_sec()
while True :
rospy.loginfo("Turtlesim rotates")
velocity_publisher.publish(velocity_message)
t1 = rospy.Time.now().to_sec()
current_angle_degree = (t1-t0)*angular_speed_degree
loop_rate.sleep()
print 'current_angle_degree: ',current_angle_degree
if (current_angle_degree>relative_angle_degree):
rospy.loginfo("reached")
break
#finally, stop the robot when the distance is moved
velocity_message.angular.z =0
velocity_publisher.publish(velocity_message)
def go_to_goal(x_goal, y_goal):
global x
global y, z, yaw
velocity_message = Twist()
cmd_vel_topic='/turtle1/cmd_vel'
while (True):
K_linear = 0.5
distance = abs(math.sqrt(((x_goal-x) ** 2) + ((y_goal-y) ** 2)))
linear_speed = distance * K_linear
K_angular = 4.0
desired_angle_goal = math.atan2(y_goal-y, x_goal-x)
angular_speed = (desired_angle_goal-yaw)*K_angular
velocity_message.linear.x = linear_speed
velocity_message.angular.z = angular_speed
velocity_publisher.publish(velocity_message)
print 'x=', x, 'y=',y
if (distance <0.01):
break
if __name__ == '__main__':
try:
rospy.init_node('turtlesim_motion_pose', anonymous=True)
#move (0.3, 0.5 , False)
time.sleep(1.0)
rotate (90, 90 , True)
except rospy.ROSInterruptException:
rospy.loginfo("node terminated.")
The entire code can be understood with the help of the comment lines
Now finally, the rosrun command can be used to run the turtle bot simulation node.
Now, we will discuss the movement of the turtlebot in the specific path from source to destination.
Moving of the Robot in the Straight Line
To run the turtle bot in the straight line, initially the roscore has to be initialized to run the master node. This is one part of robot motion in ROS
Then the turtlesim command has to be executed, to run the turtlesim node.
rosrun turtlesim turtlesim_nodeNow, the following code has to be written in the c++ IDE and has to be executed, to run the robot in the straight line.
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "turtlesim/Pose.h"
#include <sstream>
using namespace std;
ros::Publisher velocity_publisher;
ros::Subscriber pose_subscriber;
turtlesim::Pose turtlesim_pose;
const double x_min = 0.0;
const double y_min = 0.0;
const double x_max = 11.0;
const double y_max = 11.0;
const double PI = 3.14159265359;
void move(double speed, double distance, bool isForward);
void rotate (double angular_speed, double angle, bool clockwise);
double degrees2radians(double angle_in_degrees);
void setDesiredOrientation (double desired_angle_radians);
void poseCallback(const turtlesim::Pose::ConstPtr & pose_message);
void moveGoal(turtlesim::Pose goal_pose, double distance_tolerance);
void gridClean();
void spiralClean();
int main(int argc, char **argv)
{
ros::init(argc, argv, "turtlesim_cleaner");
ros::NodeHandle n;
double speed, angular_speed;
double distance, angle;
bool isForward, clockwise;
velocity_publisher = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1000);
pose_subscriber = n.subscribe("/turtle1/pose", 10, poseCallback);
/** test your code here **/
ROS_INFO("\n\n\n******START TESTING************\n");
/*cout<<"enter speed: ";
cin>>speed;
cout<<"enter distance: ";
cin>>distance;
cout<<"forward?: ";
cin>>isForward;
move(speed, distance, isForward);
cout<<"enter angular velocity (degree/sec): ";
cin>>angular_speed;
cout<<"enter desired angle (degrees): ";
cin>>angle;
cout<<"clockwise ?: ";
cin>>clockwise;
rotate(degrees2radians(angular_speed), degrees2radians(angle), clockwise);
*/
ros::Rate loop_rate(0.5);
/*setDesiredOrientation(degrees2radians(120));
loop_rate.sleep();
setDesiredOrientation(degrees2radians(-60));
loop_rate.sleep();
setDesiredOrientation(degrees2radians(0));*/
/*turtlesim::Pose goal_pose;
goal_pose.x=1;
goal_pose.y=1;
goal_pose.theta=0;
moveGoal(goal_pose, 0.01);
loop_rate.sleep();
*/
//gridClean();
ros::Rate loop(0.5);
turtlesim::Pose pose;
pose.x=1;
pose.y=1;
pose.theta=0;
moveGoal(pose, 0.01);
pose.x=6;
pose.y=6;
pose.theta=0;
moveGoal(pose, 0.01);
loop.sleep();
//spiralClean();
ros::spin();
return 0;
}
/**
* makes the robot move with a certain linear velocity for a
* certain distance in a forward or backward straight direction.
*/
void move(double speed, double distance, bool isForward){
geometry_msgs::Twist vel_msg;
//set a random linear velocity in the x-axis
if (isForward)
vel_msg.linear.x =abs(speed);
else
vel_msg.linear.x =-abs(speed);
vel_msg.linear.y =0;
vel_msg.linear.z =0;
//set a random angular velocity in the y-axis
vel_msg.angular.x = 0;
vel_msg.angular.y = 0;
vel_msg.angular.z =0;
double t0 = ros::Time::now().toSec();
double current_distance = 0.0;
ros::Rate loop_rate(100);
do{
velocity_publisher.publish(vel_msg);
double t1 = ros::Time::now().toSec();
current_distance = speed * (t1-t0);
ros::spinOnce();
loop_rate.sleep();
//cout<<(t1-t0)<<", "<<current_distance <<", "<<distance<<endl;
}while(current_distance<distance);
vel_msg.linear.x =0;
velocity_publisher.publish(vel_msg);
}
void rotate (double angular_speed, double relative_angle, bool clockwise){
geometry_msgs::Twist vel_msg;
//set a random linear velocity in the x-axis
vel_msg.linear.x =0;
vel_msg.linear.y =0;
vel_msg.linear.z =0;
//set a random angular velocity in the y-axis
vel_msg.angular.x = 0;
vel_msg.angular.y = 0;
if (clockwise)
vel_msg.angular.z =-abs(angular_speed);
else
vel_msg.angular.z =abs(angular_speed);
double current_angle = 0.0;
double t0 = ros::Time::now().toSec();
ros::Rate loop_rate(10);
do{
velocity_publisher.publish(vel_msg);
double t1 = ros::Time::now().toSec();
current_angle = angular_speed * (t1-t0);
ros::spinOnce();
loop_rate.sleep();
}while(current_angle<relative_angle);
vel_msg.angular.z =0;
velocity_publisher.publish(vel_msg);
}
double degrees2radians(double angle_in_degrees){
return angle_in_degrees *PI /180.0;
}
void setDesiredOrientation (double desired_angle_radians){
double relative_angle_radians = desired_angle_radians - turtlesim_pose.theta;
bool clockwise = ((relative_angle_radians<0)?true:false);
//cout<<desired_angle_radians <<","<<turtlesim_pose.theta<<","<<relative_angle_radians<<","<<clockwise<<endl;
rotate (degrees2radians(10), abs(relative_angle_radians), clockwise);
}
void poseCallback(const turtlesim::Pose::ConstPtr & pose_message){
turtlesim_pose.x=pose_message->x;
turtlesim_pose.y=pose_message->y;
turtlesim_pose.theta=pose_message->theta;
}
double getDistance(double x1, double y1, double x2, double y2){
return sqrt(pow((x1-x2),2)+pow((y1-y2),2));
}
void moveGoal(turtlesim::Pose goal_pose, double distance_tolerance){
geometry_msgs::Twist vel_msg;
ros::Rate loop_rate(100);
double E = 0.0;
do{
/****** Proportional Controller ******/
//linear velocity in the x-axis
double Kp=1.0;
double Ki=0.02;
//double v0 = 2.0;
//double alpha = 0.5;
double e = getDistance(turtlesim_pose.x, turtlesim_pose.y, goal_pose.x, goal_pose.y);
double E = E+e;
//Kp = v0 * (exp(-alpha)*error*error)/(error*error);
vel_msg.linear.x = (Kp*e);
vel_msg.linear.y =0;
vel_msg.linear.z =0;
//angular velocity in the z-axis
vel_msg.angular.x = 0;
vel_msg.angular.y = 0;
vel_msg.angular.z =4*(atan2(goal_pose.y-turtlesim_pose.y, goal_pose.x-turtlesim_pose.x)-turtlesim_pose.theta);
velocity_publisher.publish(vel_msg);
ros::spinOnce();
loop_rate.sleep();
}while(getDistance(turtlesim_pose.x, turtlesim_pose.y, goal_pose.x, goal_pose.y)>distance_tolerance);
cout<<"end move goal"<<endl;
vel_msg.linear.x =0;
vel_msg.angular.z = 0;
velocity_publisher.publish(vel_msg);
}
void gridClean(){
ros::Rate loop(0.5);
turtlesim::Pose pose;
pose.x=1;
pose.y=1;
pose.theta=0;
moveGoal(pose, 0.01);
loop.sleep();
setDesiredOrientation(0);
loop.sleep();
move(2.0, 9.0, true);
loop.sleep();
rotate(degrees2radians(10), degrees2radians(90), false);
loop.sleep();
move(2.0, 9.0, true);
rotate(degrees2radians(10), degrees2radians(90), false);
loop.sleep();
move(2.0, 1.0, true);
rotate(degrees2radians(10), degrees2radians(90), false);
loop.sleep();
move(2.0, 9.0, true);
rotate(degrees2radians(30), degrees2radians(90), true);
loop.sleep();
move(2.0, 1.0, true);
rotate(degrees2radians(30), degrees2radians(90), true);
loop.sleep();
move(2.0, 9.0, true);
double distance = getDistance(turtlesim_pose.x, turtlesim_pose.y, x_max, y_max);
}
void spiralClean(){
geometry_msgs::Twist vel_msg;
double count =0;
double constant_speed=4;
double vk = 1;
double wk = 2;
double rk = 0.5;
ros::Rate loop(1);
do{
rk=rk+1.0;
vel_msg.linear.x =rk;
vel_msg.linear.y =0;
vel_msg.linear.z =0;
//set a random angular velocity in the y-axis
vel_msg.angular.x = 0;
vel_msg.angular.y = 0;
vel_msg.angular.z =constant_speed;//((vk)/(0.5+rk));
cout<<"vel_msg.linear.x = "<<vel_msg.linear.x<<endl;
cout<<"vel_msg.angular.z = "<<vel_msg.angular.z<<endl;
velocity_publisher.publish(vel_msg);
ros::spinOnce();
loop.sleep();
//vk = vel_msg.linear.x;
//wk = vel_msg.angular.z;
//rk = vk/wk;
cout<<rk<<", "<<vk <<", "<<wk<<endl;
}while((turtlesim_pose.x<10.5)&&(turtlesim_pose.y<10.5));
vel_msg.linear.x =0;
velocity_publisher.publish(vel_msg);
}
The above code is explained with the comment lines, to run the turtle bot in the straight line path.
Now, finally the robot _cleaner node has to be executed to run the above code and perform the simulation
rosrun turtlesim_cleaner robot_cleaner nodeFinally, this ends with an output of the simulation of the movement of the turtlebot. For any sort of occurrence of the errors while execution or any doubts in the tutorial you can reach out through the comment box.
also read ROS