Robotics StackExchange | Archived questions

How to generate a reference closed circular path and tracking it by differential mobile robot?

How to generate a reference circular path in [x,y,theta,v,omega] and tracking it by differential mobile robot like "jackal"? and insert this reference path in gazebo and rviz?

Edit 1: I want to generate a reference closed circle and tracking it by the differential mobile robot in infinity loop.

example for two cases

My robot has an IMU 9 doF and 2 motor with its encoder and I am using odom and IMU data by robot_localization

This node generate a circular path [x,y,theta,v,omega] depend "function" on time, So "it is generated in real time", So it is varying with time.

import rospy
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Pose
import math

scale=1;
def talker():
    pub_vel = rospy.Publisher('ref_vel', Twist, queue_size=10)
    pub_pose = rospy.Publisher('ref_pose', Pose, queue_size=10)

    rospy.init_node('ref_path', anonymous=True)
    rate = rospy.Rate(10) # 10hz
    ref_vel_msg = Twist()
    ref_pose_msg = Pose()
    while not rospy.is_shutdown():
    time= rospy.get_time()
    xr = scale*math.cos(time);
    yr = scale*math.sin(time);

    xrdot = scale * -math.sin(time);
    yrdot = scale *  math.cos(time);
    xr2dot = scale * -math.cos(time);
    yr2dot = scale * -math.sin(time);

    omega=(xrdot*yr2dot-yrdot*xr2dot)/(pow(xrdot,2)+pow(yrdot,2));
    v=math.sqrt(pow(xrdot,2)+pow(yrdot,2));

    ref_pose_msg.position.x=xr;
    ref_vel_msg.linear.x=v;
    #ref_vel_msg.linear.y=0;

    ref_vel_msg.angular.z=omega;
        #hello_str = "hello world %s" % rospy.get_time()
        #rospy.loginfo(hello_str)

        pub_vel.publish(ref_vel_msg)
    pub_pose.publish(ref_pose_msg)
        rate.sleep()

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass

Asked by Ahmed_Desoky on 2020-07-19 15:55:31 UTC

Comments

With circular do you refer to an actual circle or just a closed path like a loop? You can generate a path from the odometry of the robot by accumulating poses in a node. Since the ROS path msg type is a pose array you will have [x, y, theta]. Then for V and Omega, since they are control variables you will need to calculate them by using your robot position and the nearest path pose; you can just apply the differential drive kinematics to do so. The most tipical one would be to have a kind of PI controller to track each position and calculate in each iteration the velocity and Omega of the platform.

As for the reference path in Gazebo: You do not really need it, you can just spawn the robot in an empty world an visualize it in Rviz. If what you want is generating a kind of road with the path you will need to generate a road mesh (Meshlab, blender etc.) and import it in Gazebo to have a visual of your set up,

Asked by Weasfas on 2020-07-20 06:40:36 UTC

Hi @Ahmed_Desoky,

I am not an expert but my problem with your approach is that you are assuming a perfect environment, I think that there is always a natural inclination to error when you are computing offline your control variables since you are forcing your environment to be less dynamic and autonomous.

If you are talking about a path then is not a trajectory so I am assuming that you do not want something pure reactive but planned. So for my point of view it would be better to produce the same path you are generating offline and then computing the control variables with the path and your robot pose. Since you already have a high precision odometry from the robot_localization node, It is assumed that the control values computation will be perfectly matched with the in time necessity of the control iteration.

But as I said am not really an expert in this field and I am sure there is very talented people in this community that can help more than me in this particular topic.

Asked by Weasfas on 2020-07-20 07:39:59 UTC

I want to generate a reference closed circle and tracking it by the differential mobile robot in infinity loop.

example for two cases

Asked by Ahmed_Desoky on 2020-07-20 11:35:24 UTC

Well that is fairly simple, you have a circular path published by a node as a ROS path and you have two other instances of another node that is in charge of computing the V and Omega control variables for each robot with the nearest waypoint to the subscribed path and the actual pose of the robot. If you have two robots, you will need two namespaces to organize your topics.

For generating a path you can just read a file and parse each line as an [z, y ,theta], populate a path msg and publish. Thats is how I would do it.

Asked by Weasfas on 2020-07-20 15:08:43 UTC

Answers