Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

TL;DR: Use a SimpleActionClient to send a MoveBaseGoal to an instance of the move_base node. There's a tutorial that shows you how to do this in C++ here. In the target pose you can specify a pose and an orientation. What you want to do is calculate the heading between "goalN" and "goalN+1" then send your robot to goalN with that heading. So that when the robot successfully gets to goalN it will automatically rotate on the spot to face goalN+1


Full explanation:

So you want to achieve a trajectory that looks something like this?

square_odom

I've just managed to achieve this myself, and there isn't really a good explanation anywhere of how to do all this so I'll try to quickly run you through it.

First, you need to set up your navigation stack. Doesn't really matter how you do it as long as you have a map, an odometry stream for your robot in that map, and an instance of the move_base node. I'm personally using the robot_localization package and a pre-made launch file for the move_base node that came with the robot I'm using.

Next, you want to create a node which acts as a SimpleActionClient. There is already a tutorial on how to do this in C++ here, but I'm using Python so that's what my explanation will be in. Essentially the actionlib package is a framework for writing your own goal and feedback system. The client sends the goal (usually from your laptop) and the server typically runs on the robot, executes the goal and provides feedback. There is already a simplified version of this package with a few basic goals built in, and this is what we will be using. More specifically, we want to use the 'MoveBaseAction' goal that tells the move_base node where to send the robot.

The code I use (roughly para-phrased) is here:

# Important modules to import
import rospy
import actionlib
from move_base_msgs.msg import *

# Create your SimpleActionClient and boot a default version of the SimpleActionServer
client = actionlib.SimpleActionClient('move_base', MoveBaseAction) 
client.wait_for_server()

def send_robot_to_goal(X, Y, heading):
    # Cancel any pre-existing goals (don't want them to conflict)
    self.client.cancel_all_goals()    

   # Convert to a quaternion (I'm using custom functions so treat the next line as pseudo-code) 
    [quatX, quatY, quatZ, quatW] = heading_to_quaternion(heading)

    # Create a new goal
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = 'map'                     # The frame you want to navigate in
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = float(X)                  # X coordinate to travel to
    goal.target_pose.pose.position.y = float(Y)                  # Y coordinate to travel to
    goal.target_pose.pose.orientation.x = quatX
    goal.target_pose.pose.orientation.y = quatY
    goal.target_pose.pose.orientation.z = quatZ
    goal.target_pose.pose.orientation.w = quatW

    # Send goal to move_base node
    self.client.send_goal(goal)

Then for every point you want to send the robot to - say "goalN", calculate the heading between "goalN" and 'goalN+1" and send your robot to the X,Y position of "goalN' with that heading. When the robot gets to the X,Y coordinate, it will rotate on the spot to face the heading you specified. I'm in a bit short on time so I'll stop here an try to improve my explanation soon.

TL;DR: Use a SimpleActionClient to send a MoveBaseGoal to an instance of the move_base node. There's a tutorial that shows you how to do this in C++ here. In the target pose you can specify a pose and an orientation. What you want to do is calculate the heading between "goalN" and "goalN+1" then send your robot to goalN with that heading. So that when the robot successfully gets to goalN it will automatically rotate on the spot to face goalN+1


Full explanation:

So you want to achieve a trajectory that looks something like this?

square_odom

I've just managed to achieve this myself, and there isn't really a good explanation anywhere of how to do all this so I'll try to quickly run you through it.

First, you need to set up your navigation stack. Doesn't really matter how you do it as long as you have a map, an odometry stream for your robot in that map, and an instance of the move_base node. I'm personally using the robot_localization package and a pre-made launch file for the move_base node that came with the robot I'm using.

Next, you want to create a node which acts as a SimpleActionClient. There is already a tutorial on how to do this in C++ here, but I'm using Python so that's what my explanation will be in. Essentially the actionlib package is a framework for writing your own goal and feedback system. The client sends the goal (usually from your laptop) and the server typically runs on the robot, executes the goal and provides feedback. There is already a simplified version of this package with a few basic goals built in, and this is what we will be using. More specifically, we want to use the 'MoveBaseAction' goal that tells the move_base node where to send the robot.

The code I use (roughly para-phrased) is here:

# Important modules to import
import rospy
import actionlib
from move_base_msgs.msg import *

# Create your SimpleActionClient and boot a default version of the SimpleActionServer
client = actionlib.SimpleActionClient('move_base', MoveBaseAction) 
client.wait_for_server()

def send_robot_to_goal(X, Y, heading):
    # Cancel any pre-existing goals (don't want them to conflict)
    self.client.cancel_all_goals()    

   # Convert to a quaternion (I'm using custom functions so treat the next line as pseudo-code) 
    [quatX, quatY, quatZ, quatW] = heading_to_quaternion(heading)

    # Create a new goal
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = 'map'                # The frame you want to navigate in
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = float(X)              # X coordinate to travel to
    goal.target_pose.pose.position.y = float(Y)              # Y coordinate to travel to
    goal.target_pose.pose.orientation.x = quatX
    goal.target_pose.pose.orientation.y = quatY
    goal.target_pose.pose.orientation.z = quatZ
    goal.target_pose.pose.orientation.w = quatW

    # Send goal to move_base node
    self.client.send_goal(goal)

Then for every point you want to send the robot to - say "goalN", calculate the heading between "goalN" and 'goalN+1" and send your robot to the X,Y position of "goalN' with that heading. When the robot gets to the X,Y coordinate, it will rotate on the spot to face the heading you specified. I'm in a bit short on time so I'll stop here an try to improve my explanation soon.

soon.

TL;DR: Use a SimpleActionClient to send a MoveBaseGoal to an instance of the move_base node. There's a tutorial that shows you how to do this in C++ here. or look below for the python equivalent. In the target pose you can specify a pose and an orientation. What you want to do is calculate the heading between "goalN" and "goalN+1" then send your robot to goalN with that heading. So that when the robot successfully gets to goalN it will automatically rotate on the spot to face goalN+1goalN+1.

Of course, MoveBase goals work with any heading you provide, so your robot doesn't _have_ to rotate to face anything specifically. But if your robot is already facing the next goal, that usually ensures that the path-planner will plot a straight line directly to it.


Full explanation:

So you want to achieve a trajectory that looks something like this?

square_odom

I've just managed to achieve this myself, and there isn't really a good explanation anywhere of how to do all this so I'll try to quickly run you through it.

First, you need to set up your

What you will need

  1. A navigation stack. stack for your robot. Doesn't really matter how you do it as long as you have a map, an odometry stream for your robot in that map, and an instance of the move_base node. I'm personally using the robot_localization package and a pre-made launch file for the move_base node that came with the robot I'm using.

    Next, you want to create a

  2. A node which acts as a SimpleActionClient. SimpleActionClient. You will have to write this yourself, but it's relatively straight forward as long as your navigation stack works properly.

Writing the SimpleActionClient

There is already a tutorial on how to do this in C++ here, but I'm using Python so that's what my explanation will be in. in.

Essentially the actionlib package is a framework for writing your own goal and feedback system. The client sends the goal (usually from your laptop) and the server typically runs on the robot, executes the goal and provides feedback. The goal can be anything from moving the robot base to moving an arm. There is already a simplified version of this package with a few basic goals built in, and this is what we will be using. More specifically, we want to use the 'MoveBaseAction' goal that tells the move_base node where to send the robot.robot in the world.

The code I use (roughly para-phrased) is here:

# Important modules to import
import rospy
import actionlib
from move_base_msgs.msg import *

# Create your SimpleActionClient and boot a default version of the SimpleActionServer
client = actionlib.SimpleActionClient('move_base', MoveBaseAction) 
client.wait_for_server()

def send_robot_to_goal(X, Y, heading):
    # Cancel any pre-existing goals (don't want them to conflict)
    self.client.cancel_all_goals()    

    # Convert to a heading (yaw) to quaternion
    yaw = heading
    roll = 0.0
    pitch = 0.0
    quaternion (I'm using custom functions so treat the next line as pseudo-code) 
    [quatX, quatY, quatZ, quatW] = heading_to_quaternion(heading)
= tf.transformations.quaternion_from_euler(roll, pitch, yaw)

    # Create a new goal
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = 'map'           # The frame you want to navigate in
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = float(X)         # X coordinate to travel to
    goal.target_pose.pose.position.y = float(Y)         # Y coordinate to travel to
    goal.target_pose.pose.orientation.x = quatX
quaternion[0]
    goal.target_pose.pose.orientation.y = quatY
quaternion[1]
    goal.target_pose.pose.orientation.z = quatZ
quaternion[2]
    goal.target_pose.pose.orientation.w = quatW
quaternion[3]

    # Send goal to move_base node
    self.client.send_goal(goal)

Then The heading can be whatever you want, and you can calculate it however you want. I think from memory ROS expects roll/pitch/yaw to be of the format +0 >> +180 >> -180 >> -0 instead of 0 >> 360 degrees. I prefer to work with 0 >> 360 so I've written a lot of my own functions for the heading/quaternion calculations.

X,Y are the points you want the robot to move to in the 'map' frame. This should be the same frame created by your navigation stack, and the one that all your odometry data is reported in.

Also, something to be mindful of, the heading the robot will rotate to is it's heading in the map frame, which may be different from magnetic heading depending on how you set up your navigation stack

Making move_base plot straight paths

All this by itself won't achieve anything unless you understand a little bit about how move_base works. If move_base is ordered to move the robot to a point it isn't currently facing, it will bring the robot around in a sweeping arc before heading towards the goal - as I'm sure you've already seen.

However, if the robot is already facing the goal, then it will (usually) just plot a path straight to the goal, resulting in odometry similar to the image above.

So for every point you want to send the robot to - say "goalN", calculate the heading between "goalN" and 'goalN+1" and send your robot to the X,Y position of "goalN' with that heading. When the robot gets to the X,Y coordinate, it will rotate on the spot to face the heading you specified. But the key part being that it will only rotate on the spot once you've already arrived at X,Y.

In theory of course, you can break this down into a two-part motion, a rotation and then a movement forward (translation). The rotation is just a move goal where X,Y are the current X,Y coordinates of the robot in the 'map' frame. However, personally I've found this doesn't work most of the time (the robot simply doesn't move at all), and it's something I'm in a bit short still working on time so hopefully I'll stop here an try to improve my explanation be able to update this again soon.

TL;DR: Use a SimpleActionClient to send a MoveBaseGoal to an instance of the move_base node. There's a tutorial that shows you how to do this in C++ here or look below for the python equivalent. In the target pose you can specify a pose and an orientation. What you want to do is calculate the heading between "goalN" and "goalN+1" then send your robot to goalN with that heading. So that when the robot successfully gets to goalN it will automatically rotate on the spot to face goalN+1.

Of course, MoveBase goals work with any heading you provide, so your robot doesn't _have_ have to rotate to face anything specifically. But if your robot is already facing the next goal, that usually ensures that the path-planner will plot a straight line directly to it.


Full explanation:

So you want to achieve a trajectory that looks something like this?

square_odom

I've just managed to achieve this myself, and there isn't really a good explanation anywhere of how to do all this so I'll try to quickly run you through it.

What you will need

  1. A navigation stack for your robot. Doesn't really matter how you do it as long as you have a map, an odometry stream for your robot in that map, and an instance of the move_base node. I'm personally using the robot_localization package and a pre-made launch file for the move_base node that came with the robot I'm using.
  2. A node which acts as a SimpleActionClient. You will have to write this yourself, but it's relatively straight forward as long as your navigation stack works properly.

Writing the SimpleActionClient

There is already a tutorial on how to do this in C++ here, but I'm using Python so that's what my explanation will be in.

Essentially the actionlib package is a framework for writing your own goal and feedback system. The client sends the goal (usually from your laptop) and the server typically runs on the robot, executes the goal and provides feedback. The goal can be anything from moving the robot base to moving an arm. There is already a simplified version of this package with a few basic goals built in, and this is what we will be using. More specifically, we want to use the 'MoveBaseAction' goal that tells the move_base node where to send the robot in the world.

The code I use (roughly para-phrased) is here:

# Important modules to import
import rospy
import actionlib
from move_base_msgs.msg import *

# Create your SimpleActionClient and boot a default version of the SimpleActionServer
client = actionlib.SimpleActionClient('move_base', MoveBaseAction) 
client.wait_for_server()

def send_robot_to_goal(X, Y, heading):
    # Cancel any pre-existing goals (don't want them to conflict)
    self.client.cancel_all_goals()    

    # Convert to a heading (yaw) to quaternion
    yaw = heading
    roll = 0.0
    pitch = 0.0
    quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)

    # Create a new goal
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = 'map'          # The frame you want to navigate in
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = float(X)        # X coordinate to travel to
    goal.target_pose.pose.position.y = float(Y)        # Y coordinate to travel to
    goal.target_pose.pose.orientation.x = quaternion[0]
    goal.target_pose.pose.orientation.y = quaternion[1]
    goal.target_pose.pose.orientation.z = quaternion[2]
    goal.target_pose.pose.orientation.w = quaternion[3]

    # Send goal to move_base node
    self.client.send_goal(goal)

The heading can be whatever you want, and you can calculate it however you want. I think from memory ROS expects roll/pitch/yaw to be of the format +0 >> +180 >> -180 >> -0 instead of 0 >> 360 degrees. I prefer to work with 0 >> 360 so I've written a lot of my own functions for the heading/quaternion calculations.

X,Y are the points you want the robot to move to in the 'map' frame. This should be the same frame created by your navigation stack, and the one that all your odometry data is reported in.

Also, something to be mindful of, the heading the robot will rotate to is it's heading in the map frame, which may be different from magnetic heading depending on how you set up your navigation stack

Making move_base plot straight paths

All this by itself won't achieve anything unless you understand a little bit about how move_base works. If move_base is ordered to move the robot to a point it isn't currently facing, it will bring the robot around in a sweeping arc before heading towards the goal - as I'm sure you've already seen.

However, if the robot is already facing the goal, then it will (usually) just plot a path straight to the goal, resulting in odometry similar to the image above.

So for every point you want to send the robot to - say "goalN", calculate the heading between "goalN" and 'goalN+1" and send your robot to the X,Y position of "goalN' with that heading. When the robot gets to the X,Y coordinate, it will rotate on the spot to face the heading you specified. But the key part being that it will only rotate on the spot once you've already arrived at X,Y.

In theory of course, you can break this down into a two-part motion, a rotation and then a movement forward (translation). The rotation is just a move goal where X,Y are the current X,Y coordinates of the robot in the 'map' frame. However, personally I've found this doesn't work most of the time (the robot simply doesn't move at all), and it's something I'm still working on so hopefully I'll be able to update this again soon.

TL;DR: Use a SimpleActionClient to send a MoveBaseGoal to an instance of the move_base node. There's a tutorial that shows you how to do this in C++ here or look below for the python equivalent. In the target pose you can specify a pose and an orientation. What you want to do is calculate the heading between "goalN" and "goalN+1" then send your robot to goalN with that heading. So that when the robot successfully gets to goalN it will automatically rotate on the spot to face goalN+1.

Of course, MoveBase goals work with any heading you provide, so your robot doesn't have to rotate to face anything specifically. But if your robot is already facing the next goal, that usually ensures that the path-planner will plot a straight line directly to it.


Full explanation:

So you want to achieve a trajectory that looks something like this?

square_odom

I've just managed to achieve this myself, and there isn't really a good explanation anywhere of how to do all this so I'll try to quickly run you through it.

What you will need

  1. A navigation stack for your robot. Doesn't really matter how you do it as long as you have a map, an odometry stream for your robot in that map, and an instance of the move_base node. I'm personally using the robot_localization package and a pre-made launch file for the move_base node that came with the robot I'm using.
  2. A node which acts as a SimpleActionClient. You will have to write this yourself, but it's relatively straight forward as long as your navigation stack works properly.

Writing the SimpleActionClient

There is already a tutorial on how to do this in C++ here, but I'm using Python so that's what my explanation will be in.

Essentially the actionlib package is a framework for writing your own goal and feedback system. The client sends the goal (usually from your laptop) and the server typically runs on the robot, executes the goal and provides feedback. The goal can be anything from moving the robot base to moving an arm. There is already a simplified version of this package with a few basic goals built in, and this is what we will be using. More specifically, we want to use the 'MoveBaseAction' goal that tells the move_base node where to send the robot in the world.

The code I use (roughly para-phrased) is here:

# Important modules to import
import rospy
import actionlib
from move_base_msgs.msg import *

# Create your SimpleActionClient and boot a default version of the SimpleActionServer
client = actionlib.SimpleActionClient('move_base', MoveBaseAction) 
client.wait_for_server()

def send_robot_to_goal(X, Y, heading):
    # Cancel any pre-existing goals (don't want them to conflict)
    self.client.cancel_all_goals()    

    # Convert to a heading (yaw) to quaternion
    yaw = heading
    roll = 0.0
    pitch = 0.0
    quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)

    # Create a new goal
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = 'map'          # The frame you want to navigate in
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = float(X)        # X coordinate to travel to
    goal.target_pose.pose.position.y = float(Y)        # Y coordinate to travel to
    goal.target_pose.pose.orientation.x = quaternion[0]
    goal.target_pose.pose.orientation.y = quaternion[1]
    goal.target_pose.pose.orientation.z = quaternion[2]
    goal.target_pose.pose.orientation.w = quaternion[3]

    # Send goal to move_base node
    self.client.send_goal(goal)

The heading can be whatever you want, and you can calculate it however you want. I think from memory ROS expects roll/pitch/yaw to be of the format +0 >> +180 >> -180 >> -0 instead of 0 >> 360 degrees. I prefer to work with 0 >> 360 so I've written a lot of my own functions for the heading/quaternion calculations.

X,Y are the points you want the robot to move to in the 'map' frame. This should be the same frame created by your navigation stack, and the one that all your odometry data is reported in.

Also, something to be mindful of, the heading the robot will rotate to is it's heading in the map frame, which may be different from magnetic heading depending on how you set up your navigation stack

Making move_base plot straight paths

All this by itself won't achieve anything unless you understand a little bit about how move_base works. If move_base is ordered to move the robot to a point it isn't currently facing, it will bring the robot around in a sweeping arc before heading towards the goal - as I'm sure you've already seen.

However, if the robot is already facing the goal, then it will (usually) just plot a path straight to the goal, resulting in odometry similar to the image above.

So for every point you want to send the robot to - say "goalN", calculate the heading between "goalN" and 'goalN+1" and send your robot to the X,Y position of "goalN' with that heading. When the robot gets to the X,Y coordinate, it will rotate on the spot to face the heading you specified. But the key part being that it will only rotate on the spot once you've already arrived at X,Y.

In theory of course, you can break this down into a two-part motion, a rotation and then a movement forward (translation). The rotation is just a move goal where X,Y are the current X,Y coordinates of the robot in the 'map' frame. However, personally I've found this doesn't work most of the time (the robot simply doesn't move at all), and it's something I'm still working on so hopefully I'll be able to update this again soon.

TL;DR: Use a SimpleActionClient to send a MoveBaseGoal to an instance of the move_base node. There's a tutorial that shows you how to do this in C++ here or look below for the python equivalent. In the target pose you can specify a pose and an orientation. What you want to do is calculate the heading between "goalN" and "goalN+1" then send your robot to goalN with that heading. So that when the robot successfully gets to goalN it will automatically rotate on the spot to face goalN+1.

Of course, MoveBase goals work with any heading you provide, so your robot doesn't have to rotate to face anything specifically. But if your robot is already facing the next goal, that usually ensures that the path-planner will plot a straight line directly to it.


Full explanation:

So you want to achieve a trajectory that looks something like this?

square_odom

I've just managed to achieve this myself, and there isn't really a good explanation anywhere of how to do all this so I'll try to quickly run you through it.

What you will need

  1. A navigation stack for your robot. Doesn't really matter how you do it as long as you have a map, an odometry stream for your robot in that map, and an instance of the move_base node. I'm personally using the robot_localization package and a pre-made launch file for the move_base node that came with the robot I'm using.
  2. A node which acts as a SimpleActionClient. You will have to write this yourself, but it's relatively straight forward as long as your navigation stack works properly.

Writing the SimpleActionClient

There is already a tutorial on how to do this in C++ here, but I'm using Python so that's what my explanation will be in.

Essentially the actionlib package is a framework for writing your own goal and feedback system. The client sends the goal (usually from your laptop) and the server typically runs on the robot, executes the goal and provides feedback. The goal can be anything from moving the robot base to moving an arm. There is already a simplified version of this package with a few basic goals built in, and this is what we will be using. More specifically, we want to use the 'MoveBaseAction' goal that tells the move_base node where to send the robot in the world.

The code I use (roughly para-phrased) is here:

# Important modules to import
import rospy
import actionlib
from move_base_msgs.msg import *

# Create your SimpleActionClient and boot a default version of the SimpleActionServer
client = actionlib.SimpleActionClient('move_base', MoveBaseAction) 
client.wait_for_server()

def send_robot_to_goal(X, Y, heading):
    # Cancel any pre-existing goals (don't want them to conflict)
    self.client.cancel_all_goals()    

    # Convert to a heading (yaw) to quaternion
    yaw = heading
    roll = 0.0
    pitch = 0.0
    quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)

    # Create a new goal
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = 'map'          # The frame you want to navigate in
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = float(X)        # X coordinate to travel to
    goal.target_pose.pose.position.y = float(Y)        # Y coordinate to travel to
    goal.target_pose.pose.orientation.x = quaternion[0]
    goal.target_pose.pose.orientation.y = quaternion[1]
    goal.target_pose.pose.orientation.z = quaternion[2]
    goal.target_pose.pose.orientation.w = quaternion[3]

    # Send goal to move_base node
    self.client.send_goal(goal)

The heading can be whatever you want, and you can calculate it however you want. I think from memory ROS expects roll/pitch/yaw to be of the format +0 >> +180 >> -180 >> -0 instead of 0 >> 360 degrees. I prefer to work with 0 >> 360 so I've written a lot of my own functions for the heading/quaternion calculations.

X,Y are the points you want the robot to move to in the 'map' frame. This should be the same frame created by your navigation stack, and the one that all your odometry data is reported in.

Also, something to be mindful of, the heading the robot will rotate to is it's heading in the map frame, which may be different from magnetic heading depending on how you set up your navigation stack

Making move_base plot straight paths

All this by itself won't achieve anything unless you understand a little bit about how move_base works. If move_base is ordered to move the robot to a point it isn't currently facing, it will bring the robot around in a sweeping arc before heading towards the goal - as I'm sure you've already seen.

However, if the robot is already facing the goal, then it will (usually) just plot a path straight to the goal, resulting in odometry similar to the image above.

So for above.

So, we can force the robot to travel everywhere in straight lines by making it face the next point your're going to send it to. For every point you want to send the robot to - say "goalN", calculate the heading between "goalN" and 'goalN+1" and send your robot to the X,Y position of "goalN' with that heading. When the robot gets to the X,Y coordinate, it will rotate on the spot to face the heading you specified. specified. But the key part being that it will only rotate on the spot once after you've already arrived at X,Y.

What I've described uses a single move goal to execute a translation (forward movement) and then a rotation. In theory of course, you can break this down into a two-part motion, two separate goals and reverse the order, i.e. a rotation and then a movement forward (translation). translation. The rotation is just a move goal where X,Y are the current X,Y coordinates of the robot in the 'map' frame. However, personally I've found this doesn't work most of the time (the (if told to rotate on the spot the robot simply doesn't move at all), move), and it's something I'm still working on so hopefully I'll be able to update this again soon.

TL;DR: Use a SimpleActionClient to send a MoveBaseGoal to an instance of the move_base node. There's a tutorial that shows you how to do this in C++ here or look below for the python equivalent. In the target pose you can specify a pose and an orientation. What you want to do is calculate the heading between "goalN" and "goalN+1" then send your robot to goalN with that heading. So that when the robot successfully gets to goalN it will automatically rotate on the spot to face goalN+1.

Of course, MoveBase goals work with any heading you provide, so your robot doesn't have to rotate to face anything specifically. But if your robot is already facing the next goal, that usually ensures that the path-planner will plot a straight line directly to it.


Full explanation:

So you want to achieve a trajectory that looks something like this?

square_odom

I've just managed to achieve this myself, and there isn't really a good explanation anywhere of how to do all this so I'll try to quickly run you through it.

What you will need

  1. A navigation stack for your robot. Doesn't really matter how you do it as long as you have a map, an odometry stream for your robot in that map, and an instance of the move_base node. I'm personally using the robot_localization package and a pre-made launch file for the move_base node that came with the robot I'm using.
  2. A node which acts as a SimpleActionClient. You will have to write this yourself, but it's relatively straight forward as long as your navigation stack works properly.

Writing the SimpleActionClient

There is already a tutorial on how to do this in C++ here, but I'm using Python so that's what my explanation will be in.

Essentially the actionlib package is a framework for writing your own goal and feedback system. The client sends the goal (usually from your laptop) and the server typically runs on the robot, executes the goal and provides feedback. The goal can be anything from moving the robot base to moving an arm. There is already a simplified version of this package with a few basic goals built in, and this is what we will be using. More specifically, we want to use the 'MoveBaseAction' goal that tells the move_base node where to send the robot in the world.

The code I use (roughly para-phrased) is here:

# Important modules to import
import rospy
import actionlib
from move_base_msgs.msg import *

# Create your SimpleActionClient and boot a default version of the SimpleActionServer
client = actionlib.SimpleActionClient('move_base', MoveBaseAction) 
client.wait_for_server()

def send_robot_to_goal(X, Y, heading):
    # Cancel any pre-existing goals (don't want them to conflict)
    self.client.cancel_all_goals()    

    # Convert to a heading (yaw) to quaternion
    yaw = heading
    roll = 0.0
    pitch = 0.0
    quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)

    # Create a new goal
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = 'map'          # The frame you want to navigate in
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = float(X)        # X coordinate to travel to
    goal.target_pose.pose.position.y = float(Y)        # Y coordinate to travel to
    goal.target_pose.pose.orientation.x = quaternion[0]
    goal.target_pose.pose.orientation.y = quaternion[1]
    goal.target_pose.pose.orientation.z = quaternion[2]
    goal.target_pose.pose.orientation.w = quaternion[3]

    # Send goal to move_base node
    self.client.send_goal(goal)

The heading can be whatever you want, and you can calculate it however you want. I think from memory ROS expects roll/pitch/yaw to be of the format +0 >> +180 >> -180 >> -0 instead of 0 >> 360 degrees. I prefer to work with 0 >> 360 so I've written a lot of my own functions for the heading/quaternion calculations.

X,Y are the points you want the robot to move to in the 'map' frame. This should be the same frame created by your navigation stack, and the one that all your odometry data is reported in.

Also, something to be mindful of, the heading the robot will rotate to is it's heading in the map frame, which may be different from magnetic heading depending on how you set up your navigation stack

Making move_base plot straight paths

All this by itself won't achieve anything unless you understand a little bit about how move_base works. If move_base is ordered to move the robot to a point it isn't currently facing, it will bring the robot around in a sweeping arc before heading towards the goal - as I'm sure you've already seen.

However, if the robot is already facing the goal, then it will (usually) just plot a path straight to the goal, resulting in odometry similar to the image above.

So, we can force the robot to travel everywhere in straight lines by making it face the next point your're going to send it to. For every point you want to send the robot to - say "goalN", calculate the heading between "goalN" and 'goalN+1" and send your robot to the X,Y position of "goalN' with that heading. When the robot gets to the X,Y coordinate, it will rotate on the spot to face the heading you specified. But the key part being that it will only rotate on the spot after you've already arrived at X,Y.

What I've described uses a single move goal to execute a translation (forward movement) and then a rotation. In theory of course, you can break this down into two separate goals and reverse the order, i.e. a rotation and then a translation. The rotation is just a move goal where X,Y are the current X,Y coordinates of the robot in the 'map' frame. However, personally I've found this doesn't work most of the time (if told to rotate on the spot the robot simply doesn't move), and it's something I'm still working on so hopefully I'll be able to update this again soon.

Hope this explanation helps.