@ -17,15 +17,15 @@ roslaunch stretch_core stretch_driver.launch
In a new terminal type the following commands.
```bash
cd catkin_ws/src/stretch_ros_turo tials/src/
python3 stow_command.py
cd catkin_ws/src/stretch_ros_tutor ials/src/
python stow_command.py
```
This will send a FollowJointTrajectory command to stow Stretch's arm.
### The Code
```python
#!/usr/bin/env python3
#!/usr/bin/env python
import rospy
from control_msgs.msg import FollowJointTrajectoryGoal
@ -34,38 +34,48 @@ import hello_helpers.hello_misc as hm
import time
class StowCommand(hm.HelloNode):
'''
A class that sends a joint trajectory goal to stow the Stretch's arm.
'''
def __init__ (self):
hm.HelloNode.__init__(self)
def issue_stow_command(self):
'''
Function that makes an action call and sends stow postion goal.
:param self: The self reference.
'''
stow_point = JointTrajectoryPoint()
stow_point.time_from_start = rospy.Duration(0.000)
stow_point.positions = [0.2, 0.0, 3.4]
def __init__ (self):
hm.HelloNode.__init__(self)
def issue_stow_command(self):
stow_point = JointTrajectoryPoint()
stow_point.time_from_start = rospy.Duration(0.000)
stow_point.positions = [0.2, 0.0, 3.4]
trajectory_goal = FollowJointTrajectoryGoal()
trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
trajectory_goal.trajectory.points = [stow_point]
trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
trajectory_goal.trajectory.header.frame_id = 'base_link'
trajectory_goal = FollowJointTrajectoryGoal()
trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
trajectory_goal.trajectory.points = [stow_point]
trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
trajectory_goal.trajectory.header.frame_id = 'base_link'
self.trajectory_client.send_goal(trajectory_goal)
rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal))
self.trajectory_client.wait_for_result()
self.trajectory_client.send_goal(trajectory_goal)
rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal))
self.trajectory_client.wait_for_result()
def main(self):
hm.HelloNode.main(self, 'stow_command', 'stow_command', wait_for_first_pointcloud=False)
rospy.loginfo('stowing...')
self.issue_stow_command()
time.sleep(2)
def main(self):
'''
Function that initiates stow_command function.
:param self: The self reference.
'''
hm.HelloNode.main(self, 'stow_command', 'stow_command', wait_for_first_pointcloud=False)
rospy.loginfo('stowing...')
self.issue_stow_command()
time.sleep(2)
if __name__ == '__main__':
try:
node = StowCommand()
node.main()
except KeyboardInterrupt:
rospy.loginfo('interrupt received, so shutting down')
try:
node = StowCommand()
node.main()
except KeyboardInterrupt:
rospy.loginfo('interrupt received, so shutting down')
```
### The Code Explained
@ -73,7 +83,7 @@ if __name__ == '__main__':
Now let's break the code down.
```python
#!/usr/bin/env python3
#!/usr/bin/env python
```
Every Python ROS [Node ](http://wiki.ros.org/Nodes ) will have this declaration at the top. The first line makes sure your script is executed as a Python script.
@ -156,15 +166,15 @@ roslaunch stretch_core stretch_driver.launch
In a new terminal type the following commands.
```bash
cd catkin_ws/src/stretch_ros_turo tials/src/
python3 multipoint_command.py
cd catkin_ws/src/stretch_ros_tutor ials/src/
python multipoint_command.py
```
This will send a list of JointTrajectoryPoint's to move Stretch's arm.
### The Code
```python
#!/usr/bin/env python3
#!/usr/bin/env python
import rospy
import time
@ -173,54 +183,64 @@ from trajectory_msgs.msg import JointTrajectoryPoint
import hello_helpers.hello_misc as hm
class MultiPointCommand(hm.HelloNode):
"""
A class that sends multiple joint trajectory goals to the stretch robot.
"""
def __init__ (self):
hm.HelloNode.__init__(self)
def __init__ (self):
hm.HelloNode.__init__(self)
def issue_multipoint_command(self):
point0 = JointTrajectoryPoint()
point0.positions = [0.2, 0.0, 3.4]
point0.velocities = [0.2, 0.2, 2.5]
point0.accelerations = [1.0, 1.0, 3.5]
def issue_multipoint_command(self):
"""
Function that makes an action call and sends multiple joint trajectory goals.
:param self: The self reference.
"""
point0 = JointTrajectoryPoint()
point0.positions = [0.2, 0.0, 3.4]
point0.velocities = [0.2, 0.2, 2.5]
point0.accelerations = [1.0, 1.0, 3.5]
point1 = JointTrajectoryPoint()
point1.positions = [0.3, 0.1, 2.0]
point1 = JointTrajectoryPoint()
point1.positions = [0.3, 0.1, 2.0]
point2 = JointTrajectoryPoint()
point2.positions = [0.5, 0.2, -1.0]
point2 = JointTrajectoryPoint()
point2.positions = [0.5, 0.2, -1.0]
point3 = JointTrajectoryPoint()
point3.positions = [0.6, 0.3, 0.0]
point3 = JointTrajectoryPoint()
point3.positions = [0.6, 0.3, 0.0]
point4 = JointTrajectoryPoint()
point4.positions = [0.8, 0.2, 1.0]
point4 = JointTrajectoryPoint()
point4.positions = [0.8, 0.2, 1.0]
point5 = JointTrajectoryPoint()
point5.positions = [0.5, 0.1, 0.0]
point5 = JointTrajectoryPoint()
point5.positions = [0.5, 0.1, 0.0]
trajectory_goal = FollowJointTrajectoryGoal()
trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
trajectory_goal.trajectory.points = [point0, point1, point2, point3, point4, point5]
trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
trajectory_goal.trajectory.header.frame_id = 'base_link'
self.trajectory_client.send_goal(trajectory_goal)
rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal))
self.trajectory_client.wait_for_result()
trajectory_goal = FollowJointTrajectoryGoal()
trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
trajectory_goal.trajectory.points = [point0, point1, point2, point3, point4, point5]
trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
trajectory_goal.trajectory.header.frame_id = 'base_link'
def main(self):
hm.HelloNode.main(self, 'multipoint_command', 'multipoint_command', wait_for_first_pointcloud=False)
rospy.loginfo('issuing multipoint command...')
self.issue_multipoint_command()
time.sleep(2)
self.trajectory_client.send_goal(trajectory_goal)
rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal))
self.trajectory_client.wait_for_result()
def main(self):
"""
Function that initiates the multipoint_command function.
:param self: The self reference.
"""
hm.HelloNode.main(self, 'multipoint_command', 'multipoint_command', wait_for_first_pointcloud=False)
rospy.loginfo('issuing multipoint command...')
self.issue_multipoint_command()
time.sleep(2)
if __name__ == '__main__':
try:
node = MultiPointCommand()
node.main()
except KeyboardInterrupt:
rospy.loginfo('interrupt received, so shutting down')
try:
node = MultiPointCommand()
node.main()
except KeyboardInterrupt:
rospy.loginfo('interrupt received, so shutting down')
```