Tuesday, September 12, 2017

Robot ND A2, ROS

Why we need ROS?

In the early days of desktop PC, each time we reinstall windows XP, we had to configure the hardware by loading various drivers. The same thing happens for robotics. After you get the hardware, all you need to do is write device driver for all sensors and actuators, develop a communication framework that can support for different protocols, write algorithms for perception, navigation and motion planning. then we can implement a mechanism to log our data, write our control algorithms, and don’t forget error handling.
ROS was released in 2007 by Stanford University. One of the primary motivation for developing ROS was the recognition that reseachers were constantly reinventing the wheel. Different groups were all working on custom solutions. It was difficult for them to share code and ideas, to compare results.
ROS manage these 3 complex steps by breaking them down into many small unit processes called nodes.
Basic concepts: nodes, topics(publisher, subscriber), message type, service, compute graph. pub-sub architecture.

turtlesim package

download 3GB VM, load .vmdk file, password: robo-nd, open terminal by ctrl+option+t
source /opt/ros/kinetic/setup.bash
echo "source /opt/ros/kinetic/setup.bash"  >> ~/.bashrc  # let terminal automatical load the source when opening
$ roscore  # start ros master, rosout node 
# ros version: 1.12.7
# new terminal to run a new node: turple simulator
rosrun turtlesim turtlesim_node
# another terminal to run another new node
rosrun turtlesim turtle_teleop_key
$ rosnode list
$ rostopic list
$ rostopic info /turtle1/cmd_vel 
# type: geometry_msgs/Twist
# publishers: None
# subscribers: /turtlesim
$ rosmsg info geometry_msgs/Twist 
# geometry_msgs/Vector3 linear
# float64 x,y,z
# geometry_msgs/Vector3 angular
# float64 x,y,z
$ rosed geometry_msgs Twist.msg 
# The program 'vim' can be found in the following packages:
* vim
* vim-gnome
* ...
$ rostopic echo /turtle1/cmd_vel # check message in real time

Catkin workspace

ROS provides a build and package management system: Catkin
A Catkin workspace is a top-level directory where the Catkin packages are built, modified and installed.


mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
$ catkin_init_workspace
cd ~/catkin_ws/
$ catkin_make  # build the workspace, get 2 new directories: build, devel

install simple_arm package

$ cd ~/catkin_ws/src
$ git clone https://github.com/udacity/simple_arm_01.git simple_arm
# install dependence
sudo apt-get install ros-kinetic-controller-manager
$ cd ..
$ catkin_make
source devel/setup.bash
roslaunch simple_arm robot_spawn.launch # launch ROS master and multiple nodes, the Gazebo simulator is launched.
rosdep check simple_arm # check dependency
rosdep install -i simple_arm
.launch file is written in XML format, the real files that are launched are inside launch node:
    <include file = "simple_arm/launch/xx.xml"/>
  <include file = "gazebo_ros/launch/empty_world.launch"/>

write ros node: simple_mover

  • simple_mover
  • arm_mover
  • look_away
add scripts hello,simple_mover
$ cd ~/catkin_ws/src/simple_arm/
$ mkdir scripts
$ cd scripts
$ echo '#!/bin/bash' >> hello
$ echo 'echo Hello World' >> hello
$ chmod u+x hello    # change mode to user access
$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ rosrun simple_arm hello
Hello world
$ cd ~/catkin_ws/src/simple_arm
$ cd scripts
$ touch simple_mover
$ chmod u+x simple_mover
copy provided codes into simple_mover
import math
import rospy
from std_msgs.msg import Float64
pub_j1 = rospy.Publisher ('/simple_arm/ joint_1_position_controller/command',Float64, queue_size=10) # <class 'rospy.optics.Publisher'>
rate = rospy.Rate(10)
ros message type: http://wiki.ros.org/std_msgs

check rostopic and rosnode

$ roslaunch simple_arm robot_spawn.launch
$ rostopic list # 34 topics
$ rostopic info /simple_arm/joint_1_position_controller/command
Type: std_msgs/Float64
Publishers: /arm_mover
$ rosnode list 
$ rosnode info /gazebo 
# a lot of publishers, subscribers and topics

ros node: arm_mover

This node provides the service move_arm, which allows other nodes in the system to send movement_commands.


A ROS service allows request/response communication to exist between nodes. Python implementation is like this:
service = rospy.Service('service_name', serviceClassName, handler)
the service_name is the name given to the service. The serviceClassName comes from the file name where the service definition exists. For example, the service has a definition provided in a .srv file. The handler is the name of the function or method that handles the incoming service message.
$ cd ~/catkin_ws/src/simple_arm/
$ mkdir srv
$ cd srv
$ touch GoToPosition.srv
Define a new service GoToPosition.srv,
float64 joint_1
float64 joint_2
duration time_elapsed
Service definitions contain 2 sections separated by a ‘—‘ line. The 1st section consists of 2 float64 fields, one for each of simple_arm‘s joints. The 2nd section is the service response, duration is the data type.
when to use topic? service? action?
create nodes
$ cd ~/catkin_ws
$ cd src/simple_arm/scripts
$ touch arm_mover
$ chmod u+x arm_mover
python code snippet in ‘arm_mover’
j1_publisher = rospy.Publisher ('/simple_arm/joint_1_position_controller/command', Float64, queue_size=10)
j2_publisher = rospy.Publisher ('/simple_arm/joint_2_position_controller/command', Float64, queue_size=10)
  except rospy.ROSInterruptException:
def mover_service():
    service = rospy.Service('~safe_move', GoToPosition, handle_safe_move_request)
def at_goal(pos_j1, goal_j1, pos_j2, goal_j2):
  #return true if joint positions are close to goal
def clamp_at_boundaries(requested_j1, requested_j2):
  #enforcing the minimum and maximum joint angles
def move_arm(pos_j1, pos_j2):

def handle_safe_move_request(req):
The “~” is the private namespace qualifier.
$ roslaunch simple_arm robot_spawn.launch
$ rqt_image_view /rgb_camera/image_raw
$ rosservice call /arm_mover/safe_move "joint_1: 1.57
joint_2: 1.57"  # note the line break is a must
# temporarily change paramter using rosparam
$ rosparam set /arm_mover/max_joint_2_angle 1.57 
$ rosservice call /arm_mover/safe_move "joint_1: 1.57
joint_2: 1.57"


sub1 = rospy.Subscriber("/topic_name", message_type, callback_function)
The "/topic_name" indicates which topic the Subscriber should listen to. The message_type is the type of message being published on "/topic_name".
The callback_function is the name of the function that should be called with each incoming message. Each time a message is received, it is passed as an argument to callback_function. Typically, this function is defined in your node to perform a useful action with the incoming data. Note that unlike service handler functions, the callback_function is not required to return anything.
create node script
$ cd ~/catkin_ws
$ cd src/simple_arm/scripts
$ touch look_away
$ chmod u+x look_away
copy and paste python code
class LookAway(object):
  def __init__(self):
    self.sub1 = rospy.Subscriber ('/simple_arm/joint_states', 
     JointState, self.joint_states_callback)
    self.sub2 = rospy.Subscriber ("rgb_camera/image_raw", 
          Image, self.look_away_callback)
    self.safe_move = rospy.ServiceProxy ('/arm_mover/safe_move', GoToPosition)
    self.last_position = None
    self.arm_moving = False
$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roslaunch simple_arm robot_spawn.launch
$ rqt_image_view /rgb_camera/image_raw
rosservice call /arm_mover/safe_move "joint_1: 0 
joint_2: 0"