给节点simple_mover添加代码
打开一个新的终端:
cd ~/catkin_ws/src/simple_arm/scripts/
gedit simple_mover
然后把以下代码复制到simple_mover文件中:
#!/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)
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
代码解释:
!/usr/bin/env python
import math
import rospy
rospy是针对ROS的官方的python客户端库。通过PYTHON与ROS交互,它提供了许多基础的函数。它提供了创建节点的,与话题交互,服务,参数等等的接口。参阅以下链接API文档,ROS wiki。
from std_msgs.msg import Float64
std_msgs包,import Float64,它是ROS中原始消息类型之一。 std_msgs包中也包含其它的原始数据类型。
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)
在mover函数最上面,定义了两个发布者,一个是关节1的,另一个是关节2的。queue_size参数用来定义消息丢弃之前存储在发布者队列中消息的最大数量。关于该参数的更多内容请参照链接。
rospy.init_node(‘arm_mover’)
初始化客户端节点并在主进程中注册。这里的arm_mover是节点的名字。init_node()必须在其它rospy包函数前申明。‘anonymous=True’确保节点的与其它的名字不同。
rate = rospy.Rate(10)
创建rate对象,值为10Hertz。该值限制ROS循环频路。太高的频率会占用不必要的高CPU使用率,太低的频率会导致系统总体延迟高。
start_time = 0
while not start_time:
start_time = rospy.Time.now().to_sec()
start_time用来确定已经过了多长时间。当使用模拟时间的ROS(正如我们在这里所做的那样),rospy.Time.now()最初返回0,直到从话题/clockj接收到第一条消息。这就是为什么start_time被设置并持续轮询直到返回非零值的原因。
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()
这是主循环。由于对rate.sleep()的调用,循环以大约10赫兹的频率运行。通过循环体的每次行程将导致发布两个联合命令消息。关节角度从正弦波采样,周期为10秒,幅度为[-π/ 2,+π/ 2]。当节点收到要关闭的信号(来自主进程或通过控制台窗口中的SIGINT信号)时,循环将退出。
if __name__ == '__main__':
try:
mover()
except rospy.ROSInterruptException:
pass
如果name变量设置为“main”,表示该脚本正在直接执行,会调用mover()函数。这里的try / except块非常重要,因为rospy广泛使用异常。这里捕获的特定异常是ROSInterruptException。当该节点在控制台中以SIGINT关闭,会引发此异常。如果在节点关闭之前可能需要完成某种清理工作,则可以在此完成。有关rospy异常的更多信息可以在这里找到。
运行simple_mover
cd ~/catkin_ws
source devel/setup.bash
roslaunch simple_arm robot_spawn.launch
一旦ROS主进程,Gazebo以及所有相关的节点都启动和运行了,我们就可以launch simple_mover。为了完成这个,我们需要新打开一个终端,然后执行以下命令:
cd ~/catkin_ws
source devel/setup.bash
rosrun simple_arm simple_mover