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):
$ 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.57Now 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移动到指定位置。
扫一扫
1550
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
