Ask Your Question
0

Right Angle Turn Problem

asked 2014-12-31 05:46:52 -0600

emreay gravatar image

Hi,

I'm trying to move my urdf model on a rectangular/square trajectory in rviz. In order to do that I try to broadcast certain transforms within certain time intervals using a timer. But the robot cannot turn exact 90 degrees, instead it turns a bit less. The errors are summed and the trajectory becomes useless. What is the right way to do this ? Thanks.

My transform broadcaster node:

#include<string>
#include<ros/ros.h>  
#include<tf/transform_broadcaster.h>
#include<math.h>

class rectangularTrajectory
{
    public:
    rectangularTrajectory();
    ~rectangularTrajectory();
    void moveAgv(double vx, double vy, double wth);
    void setPrevTime(ros::Time now);
    int getState();

    private:
    void timerCallback(const ros::TimerEvent&);
    void broadcastTF(tf::TransformBroadcaster *tf_broadcaster,
                    std::string device_frame,
                    double x, double y, double z,
                    double theta);
    ros::NodeHandle nh_;
    ros::Timer timer;
    tf::TransformBroadcaster broadcaster;
    double x, y, th, vx, vy, wth, dt, delta_x, delta_y, delta_th;
    ros::Time current_time, prev_time, duration;
};

rectangularTrajectory::rectangularTrajectory()
{
    ROS_INFO("Calling the constructor of the class rectangularTrajectory");
    timer = nh_.createTimer(ros::Duration(1.0), &rectangularTrajectory::timerCallback, this);
    x = 0.0;
    y = 0.0;
    th = M_PI/2;
}

rectangularTrajectory::~rectangularTrajectory()
{
    ROS_INFO("Calling the deconstructor of the class rectangularTrajectory");
}

void rectangularTrajectory::broadcastTF(tf::TransformBroadcaster *tf_broadcaster,
                 std::string device_frame,
                 double x, double y, double z,
                 double theta)
{
    // broadcast Transform from vehicle to device
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = ros::Time::now();
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = device_frame;
    odom_trans.transform.translation.x = x;
    odom_trans.transform.translation.y = y;
    odom_trans.transform.translation.z = z;
    odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(theta);
    //broadcasting the transform
    tf_broadcaster->sendTransform(odom_trans);
}

void rectangularTrajectory::moveAgv(double vx, double vy, double wth)
{
    current_time = ros::Time::now(); //set current time
    dt = (current_time - prev_time).toSec(); //time differential
    delta_x = vx*cos(th)*dt; //change in x axis in world frame
    delta_y = vx*sin(th)*dt; //change in y axis in world frame
    delta_th = wth*dt; //change in the orientation
    x += delta_x;
    y += delta_y;
    th += delta_th;
    //broadcast transforms
    broadcastTF(&broadcaster,"base_link",x,y,0,th);
    broadcastTF(&broadcaster,"wheel_left",x,y,0,th);
    broadcastTF(&broadcaster,"wheel_right",x,y,0,th);
    //set previous time
    prev_time = current_time;
    ros::spinOnce();
}

void rectangularTrajectory::setPrevTime(ros::Time now)
{
    prev_time = now.now();
}

int rectangularTrajectory::getState()
{
    int state;  
    nh_.getParam("state",state);
    return state;
}

void rectangularTrajectory::timerCallback(const ros::TimerEvent&)
{
    int new_state;
    new_state = getState()+1;
    nh_.setParam("state",new_state);
    ROS_INFO("timer triggered, new state: %d",new_state);
}

//main function
int main(int argc, char** argv) 
{
    ros::init(argc, argv, "rectangular_tf"); //node initialize
    rectangularTrajectory rectangularTrajectory_;
    rectangularTrajectory_.setPrevTime(ros::Time::now());
    while (ros::ok())
    {
        ros::spinOnce();

        while((rectangularTrajectory_.getState())%2 == 0)
        {
            rectangularTrajectory_.moveAgv(4,0,0);
        }

        while((rectangularTrajectory_.getState())%2 == 1)
        {
            rectangularTrajectory_.moveAgv(0,0,M_PI/2);
        }

    }//end of while
    return 0;
}//end of main

My launch file:

<launch>
    <!-- RVIZ -->
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find agv)/urdf.rviz" />
    <arg name="gui" default="False" />
    <param name="robot_description" textfile="$(find agv)/urdf/agv_.urdf" />
    <param name="use_gui" value="$(arg gui)"/>
    <param name="state" value="0" type="int"/>
    <!-- ROBOT STATE PUBLISHER NODE -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
    <!-- RECTANGULAR TF ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2014-12-31 13:00:53 -0600

Airuno2L gravatar image

updated 2014-12-31 13:02:12 -0600

Unfortunately there is not a way to fix this problem with out adding additional sensors. Dead reckoning will always result in the type of errors you are experiencing. Whether inaccuracies are from the error in your measurement of your wheels, the wheels slipping every now and then, etc. those little errors add up very quickly. You'll need some way to sense the robot's environment, such as a LIDAR, Kinect, ultrasonic sensors, GPS, etc.

If you did have a LIDAR on board, ROS's navigation package is capable of doing really good localization.

edit flag offensive delete link more

Comments

Thank you for your answer. But I see that your answer is about localization of real robot in real world, however I'm just trying to achieve this 'ideal' trajectory with a model in rviz so there are no wheel measurement errors and slipping. My problem is code originated.

emreay gravatar imageemreay ( 2014-12-31 14:29:49 -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

2 followers

Stats

Asked: 2014-12-31 05:46:52 -0600

Seen: 217 times

Last updated: Dec 31 '14