In this tutorial, we will learn how to use the ROS actionlib to send goals to a simulated robot arm. By the end of this tutorial, you will be able to build the following:
Real-World Applications
This project has a number of real-world applications:
- Order Fulfillment
- Factories
- Warehouses
- Manufacturing
- Kitchens
- and more!
Let’s get started!
Prerequisites
- You have completed this tutorial where you learned how to create a simulated robot arm.
Create a ROS Package
Let’s begin by creating a ROS package.
In a new terminal window, move to the src (source) folder of your workspace.
cd ~/catkin_ws/src
Now create the package.
catkin_create_pkg arm_to_goal actionlib roscpp rospy std_msgs
cd ~/catkin_ws/
catkin_make --only-pkg-with-deps arm_to_goal
Write the Code
Open a new terminal window.
Move to your package.
roscd arm_to_goal
Create a folder for the Python script we will write.
mkdir scripts
Move inside that folder.
cd scripts
Create a program.
gedit send_goal_to_arm.py
Add the following code:
#!/usr/bin/env python3
# Author: Automatic Addison https://automaticaddison.com
# Description: Uses the ROS action lib to move a robotic arm to a goal location
# Import the necessary libraries
from __future__ import print_function # Printing
import rospy # Python client library
import actionlib # ROS action library
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal # Controller messages
from std_msgs.msg import Float64 # 64-bit floating point numbers
from trajectory_msgs.msg import JointTrajectoryPoint # Robot trajectories
def move_robot_arm(joint_values):
"""
Function to move the robot arm to desired joint angles.
:param: joint_values A list of desired angles for the joints of a robot arm
"""
# Create the SimpleActionClient, passing the type of the action to the constructor
arm_client = actionlib.SimpleActionClient('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
# Wait for the server to start up and start listening for goals.
arm_client.wait_for_server()
# Create a new goal to send to the Action Server
arm_goal = FollowJointTrajectoryGoal()
# Store the names of each joint of the robot arm
arm_goal.trajectory.joint_names = ['arm_base_joint', 'shoulder_joint','bottom_wrist_joint' ,'elbow_joint', 'top_wrist_joint']
# Create a trajectory point
point = JointTrajectoryPoint()
# Store the desired joint values
point.positions = joint_values
# Set the time it should in seconds take to move the arm to the desired joint angles
point.time_from_start = rospy.Duration(3)
# Add the desired joint values to the goal
arm_goal.trajectory.points.append(point)
# Define timeout values
exec_timeout = rospy.Duration(10)
prmpt_timeout = rospy.Duration(5)
# Send a goal to the ActionServer and wait for the server to finish performing the action
arm_client.send_goal_and_wait(arm_goal, exec_timeout, prmpt_timeout)
if __name__ == '__main__':
"""
Main method.
"""
try:
# Initialize a rospy node so that the SimpleActionClient can
# publish and subscribe over ROS.
rospy.init_node('send_goal_to_arm_py')
# Move the joints of the robot arm to the desired angles in radians
move_robot_arm([-0.1, 0.5, 0.02, 0, 0])
print("Robotic arm has successfully reached the goal!")
except rospy.ROSInterruptException:
print("Program interrupted before completion.", file=sys.stderr)
Change the permissions.
chmod +x send_goal_to_arm.py
Compile the package.
cd ~/catkin_ws/
catkin_make --only-pkg-with-deps arm_to_goal
Run the Code
Open a new terminal window, and type:
cd ~/catkin_ws/
roslaunch mobile_manipulator_body mobile_manipulator_gazebo.launch
Open a new terminal window, and type the following command to move the robot to the goal location:
rosrun arm_to_goal send_goal_to_arm.py
That’s it! Keep building!
