ROS--6 Writing ROS Nodes

6.0 add a script

step1. Adding the scripts directory

In order to create a new node in python, you must first create the scripts directory within the simple_arm package, as it does not yet exist.

$ cd ~/catkin_ws/src/simple_arm/
$ mkdir scripts

step2. Creating a new script

$ cd scripts
$ echo '#!/bin/bash' >> hello
$ echo 'echo Hello World' >> hello

Step3:run the script

After setting the appropriate execution permissions on the file, rebuilding the workspace, and sourcing the newly created environment, you will be able to run the script.

$ chmod u+x hello
$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ rosrun simple_arm hello

6.1 Add a Publisher

Step1:Creating the empty simple_mover node script

$ cd ~/catkin_ws/src/simple_arm
$ cd scripts
$ touch simple_mover
$ chmod u+x simple_mover

Step2: Edit simple_mover file

Publishers allow a node to send messages to a topic

pub1 = rospy.Publisher("/topic_name", message_type, queue_size=size)

#!/usr/bin/env python

import math
import rospy
from std_msgs.msg import Float64

def mover():
    pub_j1 = rospy.Publisher('/simple_arm/joint_1_position_controller/command',
                             Float64, queue_size=10)
    pub_j2 = rospy.Publisher('/simple_arm/joint_2_position_controller/command',
                             Float64, queue_size=10)
‘’’
Initializes a client node and registers it with the master. Here “arm_mover” is the name of the node. init_node() must be called before any other rospy package functions are called. The argument anonymous=True makes sure that you always have a unique name for your node
’’’
    rospy.init_node('arm_mover')
    rate = rospy.Rate(10)
    start_time = 0

    while not start_time:
        start_time = rospy.Time.now().to_sec()

    while not rospy.is_shutdown():
        elapsed = rospy.Time.now().to_sec() - start_time
        pub_j1.publish(math.sin(2*math.pi*0.1*elapsed)*(math.pi/2))
        pub_j2.publish(math.sin(2*math.pi*0.1*elapsed)*(math.pi/2))
        rate.sleep()

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

 

Step3 Running simple_mover

Assuming that your workspace has recently been built, and it’s setup.bash has been sourced, you can launch simple_arm as follows:

$ cd ~/catkin_ws
$ roslaunch simple_arm robot_spawn.launch

Once ROS Master, Gazebo, and all of our relevant nodes are up and running, we can finally launch simple_mover. To do so, open a new terminal and type the following commands:

$ cd ~/catkin_ws
$ source devel/setup.bash
$ rosrun simple_arm simple_mover

6.2  ROS Services

6.2.1 Defining services

service = rospy.Service('service_name', serviceClassName, handler)

each service has a definition provided in an .srv file; this is a text file that provides the proper message type for both requests and responses.

The handler is the name of the function or method that handles the incoming service message. This function is called each time the service is called, and the message from the service call is passed to the handler as an argument. The handler should return an appropriate service response message.

6.2.2 Using Services

Services can be called directly from the command line, and you will see an example of this in the upcoming arm_mover classroom concepts.

On the other hand, to use a ROS service from within another node, you will define a ServiceProxy, which provides the interface for sending messages to the service:

service_proxy = rospy.ServiceProxy('service_name', serviceClassName)

One way the ServiceProxy can then be used to send requests is as follows:

msg = serviceClassNameRequest()
#update msg attributes here to have correct data
response = service_proxy(msg)

http://wiki.ros.org/rospy/Overview/Services

6.3: add a service

Namely, we still need to cover:

• Custom message generation
• Services
• Parameters
• Launch Files
• Subscribers
• Logging

step1. Creating a new service definition

As you learned earlier, an interaction with a service consists of two messages being passed. A request passed to the service, and a response received from the service. The definitions of the request and response message type are contained within .srv files living in the srv directory under the package’s root.

$ cd ~/catkin_ws/src/simple_arm/
$ mkdir srv
$ cd srv
$ touch GoToPosition.srv

“GoToPosition” 是服务类型名字

You should now edit GoToPosition.srv, so it contains the following:

float64 joint_1
float64 joint_2
---
duration time_elapsed

Service definitions always contain two sections, separated by a ‘---’ line. The first section is the definition of the request message. Here, a request consists of two float64 fields, one for each of simple_arm’s joints. The second section contains is the service response. The response contains only a single field, time_elapsed. The time_elapsed field is of type duration, and is responsible for indicating how long it took the arm to perform the movement.

 

Step2: Modifying CMakeLists.txt

In order for catkin to generate the python modules or C++ libraries which allow you to utilize messages in your code you must first modify simple_arm’s CMakeLists.txt (~/catkin_ws/src/simple_arm/CMakeLists.txt).

First, ensure that the find_package() macro lists std_msgs and message_generation as required packages. The find_package() macro should look as follows:

find_package(catkin REQUIRED COMPONENTS
        std_msgs
        message_generation
)

As the names might imply, the std_msgs package contains all of the basic message types, and message_generation is required to generate message libraries for all the supported languages (cpp, lisp, python, javascript).

Note: In your CMakeLists.txt, you may also see controller_manager listed as a required package. In actuality this package is not required. It was simply added as a means to demonstrate a build failure in the previous lesson. You may remove it from the list of REQUIRED COMPONENTS if you choose.

Next, uncomment the commented-out add_service_files() macro so it looks like this:

## Generate services in the 'srv' folder
add_service_files(
   FILES
   GoToPosition.srv
)

This tells catkin which files to generate code for.

Lastly, make sure that the generate_messages() macro is uncommented, as follows:

generate_messages(
   DEPENDENCIES
   std_msgs  # Or other packages containing msgs
)

Step3: Modifying package.xml

package.xml is responsible for defining many of the package’s properties, such as the name of the package, version numbers, authors, maintainers, and dependencies.

When rosdep is searching for these dependencies, it’s the package.xml file that is being parsed. Let’s add the message_generation and message_runtimedependencies.

Step3: Modifying package.xml
package.xml is responsible for defining many of the package’s properties, such as the name of the package, version numbers, authors, maintainers, and dependencies.
When rosdep is searching for these dependencies, it’s the package.xml file that is being parsed. Let’s add the message_generation and message_runtimedependencies.

<buildtool_depend>catkin</buildtool_depend>
  <build_depend>message_generation</build_depend>

  <run_depend>controller_manager</run_depend>
  <run_depend>effort_controllers</run_depend>
  <run_depend>gazebo_plugins</run_depend>
  <run_depend>gazebo_ros</run_depend>
  <run_depend>gazebo_ros_control</run_depend>
  <run_depend>joint_state_controller</run_depend>
  <run_depend>joint_state_publisher</run_depend>
  <run_depend>robot_state_publisher</run_depend>
  <run_depend>message_runtime</run_depend>
  <run_depend>xacro</run_depend>

You are now ready to build the package! For more information about package.xml, check out the ROS Wiki.

http://wiki.ros.org/catkin/package.xml

step4: Building the package

If you build the workspace successfully, you should now find that a python package containing a module for the new service GoToPosition has been created deep down in the devel directory.

$ cd ~/catkin_ws
$ catkin_make
$ cd devel/lib/python2.7/dist-packages
$ ls


$ env | grep PYTHONPATH

Step5: Creating the empty arm_mover node script

The steps you take to create the arm_mover node are exactly the same as the steps you took to create the simple_mover script, excepting the actual name of the script itself.

$ cd ~/catkin_ws
$ cd src/simple_arm/scripts
$ touch arm_mover
$ chmod u+x arm_mover

You can now edit the empty arm_mover script with your favorite text editor.

arm_mover

#!/usr/bin/env python

import math
import rospy
from std_msgs.msg import Float64
from sensor_msgs.msg import JointState
from simple_arm.srv import *

def at_goal(pos_j1, goal_j1, pos_j2, goal_j2):
    tolerance = .05
    result = abs(pos_j1 - goal_j1) <= abs(tolerance)
    result = result and abs(pos_j2 - goal_j2) <= abs(tolerance)
    return result

def clamp_at_boundaries(requested_j1, requested_j2):
    clamped_j1 = requested_j1
    clamped_j2 = requested_j2

    min_j1 = rospy.get_param('~min_joint_1_angle', 0)
    max_j1 = rospy.get_param('~max_joint_1_angle', 2*math.pi)
    min_j2 = rospy.get_param('~min_joint_2_angle', 0)
    max_j2 = rospy.get_param('~max_joint_2_angle', 2*math.pi)

    if not min_j1 <= requested_j1 <= max_j1:
        clamped_j1 = min(max(requested_j1, min_j1), max_j1)
        rospy.logwarn('j1 is out of bounds, valid range (%s,%s), clamping to: %s',
                      min_j1, max_j1, clamped_j1)

    if not min_j2 <= requested_j2 <= max_j2:
        clamped_j2 = min(max(requested_j2, min_j2), max_j2)
        rospy.logwarn('j2 is out of bounds, valid range (%s,%s), clamping to: %s',
                      min_j2, max_j2, clamped_j2)

    return clamped_j1, clamped_j2

def move_arm(pos_j1, pos_j2):
    time_elapsed = rospy.Time.now()
    j1_publisher.publish(pos_j1)
    j2_publisher.publish(pos_j2)

    while True:
        joint_state = rospy.wait_for_message('/simple_arm/joint_states', JointState)
        if at_goal(joint_state.position[0], pos_j1, joint_state.position[1], pos_j2):
            time_elapsed = joint_state.header.stamp - time_elapsed
            break

    return time_elapsed

def handle_safe_move_request(req):
    rospy.loginfo('GoToPositionRequest Received - j1:%s, j2:%s',
                   req.joint_1, req.joint_2)
    clamp_j1, clamp_j2 = clamp_at_boundaries(req.joint_1, req.joint_2)
    time_elapsed = move_arm(clamp_j1, clamp_j2)

    return GoToPositionResponse(time_elapsed)

def mover_service():
    rospy.init_node('arm_mover')
    service = rospy.Service('~safe_move', GoToPosition, handle_safe_move_request)
    rospy.spin()

if __name__ == '__main__':
    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)

    try:
        mover_service()
    except rospy.ROSInterruptException:
        pass

step6:  Launch and Interact

First, Launching the project with the new service

To get the arm_mover node, and accompanying safe_move service to launch along with all of the other nodes, you will modify robot_spawn.launch.

Launch files, when they exist, are located within the launch directory in the root of a catkin package. simple_arm’s launch file is located in ~/catkin_ws/src/simple_arm/launch

To get the arm_mover node to launch, simply add the following:

<!-- The arm mover node -->
  <node name="arm_mover" type="arm_mover" pkg="simple_arm">
    <rosparam>
      min_joint_1_angle: 0
      max_joint_1_angle: 1.57
      min_joint_2_angle: 0
      max_joint_2_angle: 1.0
    </rosparam>
  </node>

More information on the format of the launch file can be found

http://wiki.ros.org/roslaunch/XML

step7 Testing the new service

Now that you've modified the launch file, you are ready to test everything out.

First. To do so, launch the simple_arm, verify that the arm_mover node is running, and that the safe_move service is listed:

Note: You will need to make sure that you've exited out of your previous roslaunch session before re-launching.

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roslaunch simple_arm robot_spawn.launch

Then, in a new terminal, verify that the node and service have indeed launched.

$ rosnode list
$ rosservice list

To view the camera image stream, you can use the command rqt_image_view (you can learn more about rqt and the associated tools here):

http://wiki.ros.org/rqt

$ rqt_image_view /rgb_camera/image_raw

Adjusting the view

The camera is displaying a gray image. This is as to be expected, given that it is straight up, towards the gray sky of our gazebo world.

To point the camera towards the numbered blocks on the counter top, we would need to rotate both joint 1 and joint 2 by approximately pi/2 radians. Let’s give it a try:

$ cd ~/catkin_ws/
$ source devel/setup.bash
$ rosservice call /arm_mover/safe_move "joint_1: 1.57
joint_2: 1.57"

Note: rosservice call can tab-complete the request message, so that you don’t have to worry about writing it out by hand. Also, be sure to include a line break between the two joint parameters.

What was not expected is the resulting position of the arm. Looking at the roscore console, we can very clearly see what the problem was. The requested angle for joint 2 was out of the safe bounds. We requested 1.57 radians, but the maximum joint angle was set to 1.0 radians.

By setting the max_joint_2_angle on the parameter server, we should be able to bring the blocks into view the next time a service call is made. To increase joint 2’s maximum angle, you can use the command rosparam

$ rosparam set /arm_mover/max_joint_2_angle 1.57

Now we should be able to move the arm such that all of the blocks are within the field of view of the camera:

rosservice call /arm_mover/safe_move "joint_1: 1.57
joint_2: 1.57"

And there you have it. All of the blocks are within the field of view!

6.4 add Subscribers

添加一个订阅者节点，根据从主题获取的数据，做相应处理，当出现机械臂指向天空时，发送一个service消息，把机械臂移动到指定位置。

也包括一个发送服务消息

A Subscriber enables your node to read messages from a topic, allowing useful data to be streamed into the node. 

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.

More in http://docs.ros.org/api/rospy/html/rospy.topics.Subscriber-class.html

创建一个look_away节点， look_away 节点订阅/rgb_camera/image_raw 主题，该主题可以从robotic arm获取摄像头数据。

Step1: Creating the empty look_away node scrip

$ cd ~/catkin_ws
$ cd src/simple_arm/scripts
$ touch look_away
$ chmod u+x look_away

Note: If look_away starts before the system has fully initialized, then look_away hangs in the call to safe_move. 

解决办法：wait_for_message

def __init__(self):
    rospy.init_node('look_away')
    self.last_position = None
    self.arm_moving = False

    rospy.wait_for_message('/simple_arm/joint_states', JointState)
    rospy.wait_for_message('/rgb_camera/image_raw', Image)

    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)
    rospy.spin()

step2: Updating the launch file

<!-- The look away node -->
  <node name="look_away" type="look_away" pkg="simple_arm"/>

修改默认值

<!-- The arm mover node -->
  <node name="arm_mover" type="arm_mover" pkg="simple_arm">
    <rosparam>
      min_joint_1_angle: 0
      max_joint_1_angle: 1.57
      min_joint_2_angle: 0
      max_joint_2_angle: 1.57
    </rosparam>
  </node>

step3: Look_up

#!/usr/bin/env python

import math
import rospy
from sensor_msgs.msg import Image, JointState
from simple_arm.srv import *


class LookAway(object):
    def __init__(self):
        rospy.init_node('look_away')

        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

        rospy.spin()

    def uniform_image(self, image):
        return all(value == image[0] for value in image)

    def coord_equal(self, coord_1, coord_2):
        if coord_1 is None or coord_2 is None:
            return False
        tolerance = .0005
        result = abs(coord_1[0] - coord_2[0]) <= abs(tolerance)
        result = result and abs(coord_1[1] - coord_2[1]) <= abs(tolerance)
        return result

    def joint_states_callback(self, data):
        if self.coord_equal(data.position, self.last_position):
            self.arm_moving = False
        else:
            self.last_position = data.position
            self.arm_moving = True

    def look_away_callback(self, data):
        if not self.arm_moving and self.uniform_image(data.data):
            try:
                rospy.wait_for_service('/arm_mover/safe_move')
                msg = GoToPositionRequest()
                msg.joint_1 = 1.57
                msg.joint_2 = 1.57
                response = self.safe_move(msg)

                rospy.logwarn("Camera detecting uniform image. \
                               Elapsed time to look at something nicer:\n%s", 
                               response)

            except rospy.ServiceException, e:
                rospy.logwarn("Service call failed: %s", e)



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

step4: Launch and Interact

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roslaunch simple_arm robot_spawn.launch

After launching, the arm should move away from the grey sky and look towards the blocks. To view the camera image stream, you can use the same command as before:

观察图片数据

$ rqt_image_view /rgb_camera/image_raw

发送消息操作simple_arm

rosservice call /arm_mover/safe_move "joint_1: 0
joint_2: 0"

Note：

1.对safe_move发送一个service消息，移动simple_arm，移动的相关信息会发布在主题“joint_states”和“rgb_camera/image_raw”；

2.look_away节点作为订阅者节点，从这两个主题中获取位置和图像信息，当发现摄像头朝向天空时，发送一个service消息，将simple_arm移动到指定位置。