Ask Your Question
0

Cannot able to rotate turtlesim

asked 2019-01-28 17:13:18 -0600

mustafaugur gravatar image

Hi everyone, I am trying to write simple code to rotate Turtlesim robot with the folllowing code but it is not working with double values, let me explain it, I wrote this code to move straight and rotate:

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "turtlesim/Pose.h"
#include <sstream>

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



int main(int argc, char **argv)
{
    double speed, angular_speed, angular_speed_degree;
    double distance, angle, angle_degree;
    bool isForward, clockwise;

    ros::init(argc, argv,"robot_cleaner");
    ros::NodeHandle n;

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

    std::cout << "Enter the speed: ";
    std::cin >> speed;
    std::cout << "Enter the distance: ";
    std::cin >> distance;
    std::cout << "Forward?";
    std::cin >> isForward;

    move(speed, distance, isForward);

    std::cout << "Enter angular speed:";
    std::cin >> angular_speed_degree;

    angular_speed = angular_speed_degree * 3.14 /180; // Degrees to Radians

    std::cout << "Enter desired angle:";
    std::cin >> angle_degree;

    angle = angle_degree * 3.14/ 180; // Degrees to Radians

    std::cout << "Clockwise?";
    std::cin >> clockwise;


    rotate(angular_speed,angle,clockwise);

    ros::spin();

    return 0;
}

The move() function works well, but in rotate function I have problems. If I wrote parameters by myself It work well for example:

std::cout << "Enter angular speed:";
std::cin >> angular_speed_degree;
std::cout << "Enter desired angle:";
std::cin >> angle_degree;
std::cout << "Clockwise?";
std::cin >> clockwise;
rotate(angular_speed,angle,clockwise);

This code works well but, when I try to make a conversion like above code, degree to radian rotate() function fails, the rotate function is in below:

void rotate(double angular_speed, double angle, bool clockwise)
{
    // angle = angular_speed * time
    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 angular velocity
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);

// t0 is the current time
double t0 = ros::Time::now().toSec();

double current_angle = 0;

ros::Rate loop_rate(10);

// loop to publish the velocity 
// estimate, current_distance = velocity * (t1 - t0)
do
{
    // Publish the velocity
    velocity_publisher.publish(vel_msg);
    // t1 is the current time
    double t1 = ros::Time::now().toSec();
    // Calculate current_distance
    current_angle = angular_speed * (t1 - t0) ;
    ros::spinOnce();
    loop_rate.sleep();
} while (current_angle < angle);

// set velocity to zero to stop the robot
vel_msg.angular.z = 0.0;
velocity_publisher.publish(vel_msg);

}

Could you please help me to find the reason of this problem, please?

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
1

answered 2019-01-31 16:51:46 -0600

Alberto E. gravatar image

Hi!

I've tested your code (the rotation part) and it works nice except for 1 bug, here:

if(clockwise)
    vel_msg.angular.z = -abs(angular_speed);
else
    vel_msg.angular.z = abs(angular_speed);

These abs() functions will return 0 for all values between 0 and +-1. So, for instance, if the angular velocity is 0.5 rad/s, you will get a 0 there, thus the robot won't rotate. It can be easily solved though, just removing the abs() functions:

if(clockwise)
    vel_msg.angular.z = -angular_speed;
else
    vel_msg.angular.z = angular_speed;

It should work perfect with this change. You can check the following post if you want to see how I tested your code.

Cheers!

edit flag offensive delete link more

Comments

1

If abs() function is really needed, its possible to use fabs() function. fabs() is like float abs(), so, return float numbers. fabs(-1.2) = 1.2 fabs(-0.2) = 0.2

Teo Cardoso gravatar image Teo Cardoso  ( 2019-01-31 19:13:51 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-01-28 17:13:18 -0600

Seen: 819 times

Last updated: Jan 31 '19