Browse Source

Added more comments.

pull/14/head
Alan G. Sanchez 2 years ago
parent
commit
99f84aebe5
15 changed files with 114 additions and 108 deletions
  1. +5
    -4
      example_2.md
  2. +1
    -1
      example_3.md
  3. +6
    -5
      example_4.md
  4. +1
    -1
      example_5.md
  5. +1
    -1
      follow_joint_trajectory.md
  6. +14
    -14
      src/avoider.py
  7. +1
    -2
      src/effort_sensing.py
  8. +6
    -6
      src/joint_state_printer.py
  9. +15
    -11
      src/marker.py
  10. +13
    -13
      src/move.py
  11. +15
    -16
      src/multipoint_command.py
  12. +15
    -12
      src/scan_filter.py
  13. +8
    -8
      src/single_joint_actuator.py
  14. +1
    -1
      src/stored_data_plotter.py
  15. +12
    -13
      src/stow_command.py

+ 5
- 4
example_2.md View File

@ -79,7 +79,7 @@ from numpy import linspace, inf
from math import sin
from sensor_msgs.msg import LaserScan
class Scanfilter:
class ScanFilter:
"""
A class that implements a LaserScan filter that removes all of the points
that are not infront of the robot
@ -89,6 +89,7 @@ class Scanfilter:
self.extent = self.width / 2.0
self.sub = rospy.Subscriber('/scan', LaserScan, self.callback)
self.pub = rospy.Publisher('filtered_scan', LaserScan, queue_size=10)
rospy.loginfo("Publishing the filtered_scan topic. Use RViz to visualize.")
def callback(self,msg):
"""
@ -106,7 +107,7 @@ class Scanfilter:
if __name__ == '__main__':
rospy.init_node('scan_filter')
Scanfilter()
ScanFilter()
rospy.spin()
```
@ -168,11 +169,11 @@ Substitute in the new ranges in the original message, and republish it.
```python
rospy.init_node('scan_filter')
Scanfilter()
ScanFilter()
```
The next line, rospy.init_node(NAME, ...), is very important as it tells rospy the name of your node -- until rospy has this information, it cannot start communicating with the ROS Master. In this case, your node will take on the name talker. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/".
Setup Scanfilter class with `Scanfilter()`
Instantiate the class with `ScanFilter()`
```python
rospy.spin()

+ 1
- 1
example_3.md View File

@ -169,7 +169,7 @@ rospy.spin()
```
The next line, rospy.init_node(NAME, ...), is very important as it tells rospy the name of your node -- until rospy has this information, it cannot start communicating with the ROS Master. In this case, your node will take on the name talker. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/".
Setup Avoider class with `Avioder()`
Instantiate class with `Avioder()`
Give control to ROS with `rospy.spin()`. This will allow the callback to be called whenever new messages come in. If we don't put this line in, then the node will not work, and ROS will not process any messages.

+ 6
- 5
example_4.md View File

@ -53,6 +53,7 @@ class Balloon():
self.marker.pose.position.x = 0.0
self.marker.pose.position.y = 0.0
self.marker.pose.position.z = 2.0
rospy.loginfo("Publishing the balloon topic. Use RViz to visualize.")
def publish_marker(self):
"""
@ -66,11 +67,11 @@ class Balloon():
if __name__ == '__main__':
rospy.init_node('marker', argv=sys.argv)
ballon = Balloon()
balloon = Balloon()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
ballon.publish_marker()
balloon.publish_marker()
rate.sleep()
```
@ -151,20 +152,20 @@ Publish the Marker data structure to be visualized in RViz.
```python
rospy.init_node('marker', argv=sys.argv)
ballon = Balloon()
balloon = Balloon()
rate = rospy.Rate(10)
```
The next line, rospy.init_node(NAME, ...), is very important as it tells rospy the name of your node -- until rospy has this information, it cannot start communicating with the ROS Master. In this case, your node will take on the name talker. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/".
Setup Balloon class with `Balloon()`
Instantiate class with `Balloon()`
Give control to ROS with `rospy.spin()`. This will allow the callback to be called whenever new messages come in. If we don't put this line in, then the node will not work, and ROS will not process any messages.
```python
while not rospy.is_shutdown():
ballon.publish_marker()
balloon.publish_marker()
rate.sleep()
```

+ 1
- 1
example_5.md View File

@ -137,7 +137,7 @@ rospy.sleep(.1)
```
The next line, rospy.init_node(NAME, ...), is very important as it tells rospy the name of your node -- until rospy has this information, it cannot start communicating with the ROS Master. In this case, your node will take on the name talker. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/".
Setup `JointStatePublisher()` class as *JSP*
Declare object, *JSP*, from the `JointStatePublisher` class.
The use of the `rospy.sleep()` function is to allow the *JSP* class to initialize all of its features before requesting to publish joint positions of desired joints (running the `print_states()` function).

+ 1
- 1
follow_joint_trajectory.md View File

@ -152,7 +152,7 @@ if __name__ == '__main__':
rospy.loginfo('interrupt received, so shutting down')
```
Initialize the `StowCommand()` class and set it to *node* and run the `main()` function.
Declare object, *node*, from the `StowCommand()` class. Then run the `main()` function.
## Multipoint Command Example

+ 14
- 14
src/avoider.py View File

@ -5,10 +5,10 @@ import rospy
from numpy import linspace, inf, tanh
from math import sin
# The Twist message is used to send velocities to the robot.
# The Twist message is used to send velocities to the robot
from geometry_msgs.msg import Twist
# We're going to subscribe to a LaserScan message.
# We're going to subscribe to a LaserScan message
from sensor_msgs.msg import LaserScan
class Avoider:
@ -23,7 +23,7 @@ class Avoider:
"""
# Set up a publisher and a subscriber. We're going to call the subscriber
# "scan", and filter the ranges similar to what we did in example 2.
# For the publisher, we're going to use the topic name /stretch/cmd_vel.
# For the publisher, we're going to use the topic name /stretch/cmd_vel
self.pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=1) #/stretch_diff_drive_controller/cmd_vel for gazebo
self.sub = rospy.Subscriber('/scan', LaserScan, self.set_speed)
@ -34,11 +34,11 @@ class Avoider:
self.extent = self.width / 2.0
# We want the robot to drive foward or backwards until it is 0.5 m from
# the closest obstacle measured in front of it.
# the closest obstacle measured in front of it
self.distance = 0.5
# Alocate a Twist to use, and set everything to zero. We're going to do
# this here, to save some time in the callback function, set_speed().
# this here, to save some time in the callback function, set_speed()
self.twist = Twist()
self.twist.linear.x = 0.0
self.twist.linear.y = 0.0
@ -48,7 +48,7 @@ class Avoider:
self.twist.angular.z = 0.0
# Called every time we get a LaserScan message from ROS.
# Called every time we get a LaserScan message from ROS
def set_speed(self,msg):
"""
Callback function to deal with incoming laserscan messages.
@ -58,33 +58,33 @@ class Avoider:
:publishes self.twist: Twist message.
"""
# Figure out the angles of the scan. We're going to do this each time, in case we're subscribing to more than one
# laser, with different numbers of beams.
# laser, with different numbers of beams
angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
# Work out the y coordinates of the ranges.
# Work out the y coordinates of the ranges
points = [r * sin(theta) if (theta < -2.5 or theta > 2.5) else inf for r,theta in zip(msg.ranges, angles)]
# If we're close to the x axis, keep the range, otherwise use inf, which means "no return".
# If we're close to the x axis, keep the range, otherwise use inf, which means "no return"
new_ranges = [r if abs(y) < self.extent else inf for r,y in zip(msg.ranges, points)]
# Calculate the difference of the closest measured scan and where we want the robot to stop.
# Calculate the difference of the closest measured scan and where we want the robot to stop
error = min(new_ranges) - self.distance
# Using hyperbolic tanget for speed regulation, with a threshold to stop
# and driving when it is close to the desired distance.
# and driving when it is close to the desired distance
self.twist.linear.x = tanh(error) if (error > 0.05 or error < -0.05) else 0
# Publish the command using the publisher
self.pub.publish(self.twist)
if __name__ == '__main__':
# Initialize the node, and call it "avoider".
# Initialize the node, and call it "avoider"
rospy.init_node('avoider')
# Setup Avoider class
# Instantiate he Avoider class
Avoider()
# Give control to ROS. This will allow the callback to be called whenever new
# messages come in. If we don't put this line in, then the node will not work,
# and ROS will not process any messages.
# and ROS will not process any messages
rospy.spin()

+ 1
- 2
src/effort_sensing.py View File

@ -172,8 +172,7 @@ class JointActuatorEffortSensor(hm.HelloNode):
if __name__ == '__main__':
try:
# Initialize the SingleJointActuator() class and set it to node and run the
# main() function.
# Declare object, node, from JointActuatorEffortSensor class
node = JointActuatorEffortSensor()
node.main()
except KeyboardInterrupt:

+ 6
- 6
src/joint_state_printer.py View File

@ -5,7 +5,7 @@ import rospy
import sys
# We're going to subscribe to 64-bit integers, so we need to import the definition
# for them.
# for them
from sensor_msgs.msg import JointState
class JointStatePublisher():
@ -43,7 +43,7 @@ class JointStatePublisher():
# Use of forloop to parse the names of the requested joints list.
# The index() function returns the index at the first occurrence of
# the name of the requested joint in the self.joint_states.name list.
# the name of the requested joint in the self.joint_states.name list
for joint in joints:
index = self.joint_states.name.index(joint)
joint_positions.append(self.joint_states.position[index])
@ -64,21 +64,21 @@ if __name__ == '__main__':
# Initialize the node.
rospy.init_node('joint_state_printer', anonymous=True)
# Set JointStatePublisher to JSP
# Declare object from JointStatePublisher class
JSP = JointStatePublisher()
# Use the rospy.sleep() function to allow the class to initialize before
# requesting to publish joint_positions of desired joints (running the
# print_states() function).
# print_states() function)
rospy.sleep(.1)
# Create a list of the joints and name them joints. These will be an argument
# for the print_states() function.
# for the print_states() function
joints = ["joint_lift", "joint_arm_l0", "joint_arm_l1", "joint_arm_l2", "joint_arm_l3", "joint_wrist_yaw"]
# joints = ["joint_head_pan","joint_head_tilt", "joint_gripper_finger_left", "joint_gripper_finger_right"]
JSP.print_states(joints)
# Give control to ROS. This will allow the callback to be called whenever new
# messages come in. If we don't put this line in, then the node will not work,
# and ROS will not process any messages.
# and ROS will not process any messages
rospy.spin()

+ 15
- 11
src/marker.py View File

@ -15,14 +15,14 @@ class Balloon():
Function that initializes the marker's features.
:param self: The self reference.
"""
# Set up a publisher. We're going to publish on a topic called balloon.
# Set up a publisher. We're going to publish on a topic called balloon
self.pub = rospy.Publisher('balloon', Marker, queue_size=10)
# Create a marker. Markers of all shapes share a common type.
self.marker = Marker()
# Set the frame ID and type. The frame ID is the frame in which the position of the marker
# is specified. The type is the shape of the marker, detailed on the wiki page.
# is specified. The type is the shape of the marker, detailed on the wiki page
self.marker.header.frame_id = '/base_link'
self.marker.header.stamp = rospy.Time()
self.marker.type = self.marker.SPHERE
@ -30,10 +30,10 @@ class Balloon():
# Each marker has a unique ID number. If you have more than one marker that
# you want displayed at a given time, then each needs to have a unique ID
# number.If you publish a new marker with the same ID number and an existing
# marker, it will replace the existing marker with that ID number.
# marker, it will replace the existing marker with that ID number
self.marker.id = 0
# Set the action. We can add, delete, or modify markers.
# Set the action. We can add, delete, or modify markers
self.marker.action = self.marker.ADD
# These are the size parameters for the marker. These will vary by marker type
@ -41,23 +41,26 @@ class Balloon():
self.marker.scale.y = 0.5
self.marker.scale.z = 0.5
# Color, as an RGB triple, from 0 to 1.
# Color, as an RGB triple, from 0 to 1
self.marker.color.r = 1.0
self.marker.color.g = 0.0
self.marker.color.b = 0.0
# Alpha value, from 0 (invisible) to 1 (opaque). If you don't set this and
# it defaults to zero, then your marker will be invisible.
# it defaults to zero, then your marker will be invisible
self.marker.color.a = 1.0
# Specify the pose of the marker. Since spheres are rotationally invarient,
# we're only going to specify the positional elements. As usual, these are
# in the coordinate frame named in frame_id. In this case, the position
# will always be directly above the robot, and will move with it.
# will always be directly above the robot, and will move with it
self.marker.pose.position.x = 0.0
self.marker.pose.position.y = 0.0
self.marker.pose.position.z = 2.0
# Create log message
rospy.loginfo("Publishing the balloon topic. Use RViz to visualize.")
def publish_marker(self):
"""
@ -74,12 +77,13 @@ if __name__ == '__main__':
# Initialize the node, as usual
rospy.init_node('marker')
ballon = Balloon()
# Declare object from the Balloon class
balloon = Balloon()
# Set a rate.
# Set a rate
rate = rospy.Rate(10)
# Publish the marker at 10Hz.
# Publish the marker at 10Hz
while not rospy.is_shutdown():
ballon.publish_marker()
balloon.publish_marker()
rate.sleep()

+ 13
- 13
src/move.py View File

@ -3,7 +3,7 @@
# Every python controller needs these lines
import rospy
# The Twist message is used to send velocities to the robot.
# The Twist message is used to send velocities to the robot
from geometry_msgs.msg import Twist
class Move:
@ -16,7 +16,7 @@ class Move:
:param self: The self reference
"""
# Setup a publisher that will send the velocity commands for the Stretch
# This will publish on a topic called "/stretch/cmd_vel" with a message type Twist.
# This will publish on a topic called "/stretch/cmd_vel" with a message type Twist
self.pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=1) #/stretch_diff_drive_controller/cmd_vel for gazebo
@ -25,21 +25,21 @@ class Move:
Function that publishes Twist messages
:param self: The self reference.
:publishes command: Twist message
:publishes command: Twist message.
"""
# Make a Twist message. We're going to set all of the elements, since we
# can't depend on them defaulting to safe values.
# can't depend on them defaulting to safe values
command = Twist()
# A Twist has three linear velocities (in meters per second), along each of the axes.
# For Stretch, it will only pay attention to the x velocity, since it can't
# directly move in the y direction or the z direction.
# directly move in the y direction or the z direction
command.linear.x = 0.1
command.linear.y = 0.0
command.linear.z = 0.0
# A Twist also has three rotational velocities (in radians per second).
# The Stretch will only respond to rotations around the z (vertical) axis.
# The Stretch will only respond to rotations around the z (vertical) axis
command.angular.x = 0.0
command.angular.y = 0.0
command.angular.z = 0.0
@ -48,24 +48,24 @@ class Move:
self.pub.publish(command)
if __name__ == '__main__':
# Initialize the node, and call it "move".
# Initialize the node, and call it "move"
rospy.init_node('move')
# Setup Move class to base_motion
# Declare object from Move class
base_motion = Move()
# Rate allows us to control the (approximate) rate at which we publish things.
# For this example, we want to publish at 10Hz.
# For this example, we want to publish at 10Hz
rate = rospy.Rate(10)
# This will loop until ROS shuts down the node. This can be done on the
# command line with a ctrl-C, or automatically from roslaunch.
# command line with a ctrl-C, or automatically from roslaunch
while not rospy.is_shutdown():
# Run the move_foward function in the Move class
base_motion.move_forward()
# Do an idle wait to control the publish rate. If we don't control the
# Do an idle wait to control the publish rate. If we don't control the
# rate, the node will publish as fast as it can, and consume all of the
# available CPU resources. This will also add a lot of network traffic,
# possibly slowing down other things.
# available CPU resources. This will also add a lot of network traffic,
# possibly slowing down other things
rate.sleep()

+ 15
- 16
src/multipoint_command.py View File

@ -5,14 +5,14 @@ import rospy
import time
# Import the FollowJointTrajectoryGoal from the control_msgs.msg package to
# control the Stretch robot.
# control the Stretch robot
from control_msgs.msg import FollowJointTrajectoryGoal
# Import JointTrajectoryPoint from the trajectory_msgs package to define
# robot trajectories.
# robot trajectories
from trajectory_msgs.msg import JointTrajectoryPoint
# Import hello_misc script for handling trajecotry goals with an action client.
# Import hello_misc script for handling trajecotry goals with an action client
import hello_helpers.hello_misc as hm
class MultiPointCommand(hm.HelloNode):
@ -20,7 +20,7 @@ class MultiPointCommand(hm.HelloNode):
A class that sends multiple joint trajectory goals to the stretch robot.
"""
# Initialize the inhereted hm.Hellonode class.
# Initialize the inhereted hm.Hellonode class
def __init__(self):
hm.HelloNode.__init__(self)
@ -30,27 +30,27 @@ class MultiPointCommand(hm.HelloNode):
to the joint_lift, wrist_extension, and joint_wrist_yaw.
:param self: The self reference.
"""
# Set point0 as a JointTrajectoryPoint().
# Set point0 as a JointTrajectoryPoint()
point0 = JointTrajectoryPoint()
# Provide desired positions of lift, wrist extension, and yaw of
# the wrist (in meters).
# the wrist (in meters)
point0.positions = [0.2, 0.0, 3.4]
# Provide desired velocity of the lift (m/s), wrist extension (m/s),
# and wrist yaw (rad/s).
# and wrist yaw (rad/s)
# IMPORTANT NOTE: The lift and wrist extension can only go up to 0.2 m/s!
point0.velocities = [0.2, 0.2, 2.5]
# Provide desired velocity of the lift (m/s^2), wrist extension (m/s^2),
# and wrist yaw (rad/s^2).
# and wrist yaw (rad/s^2)
point0.accelerations = [1.0, 1.0, 3.5]
# Set positions for the following 5 trajectory points.
# IMPORTANT NOTE: If you do not provide any velocities or accelerations for the lift
# or wrist extension, then they go to their default values. However, the
# Velocity and Acceleration of the wrist yaw will stay the same from the
# previous value unless updated.
# previous value unless updated
point1 = JointTrajectoryPoint()
point1.positions = [0.3, 0.1, 2.0]
@ -67,7 +67,7 @@ class MultiPointCommand(hm.HelloNode):
point5.positions = [0.5, 0.1, 0.0]
# Set trajectory_goal as a FollowJointTrajectoryGoal and define
# the joint names as a list.
# the joint names as a list
trajectory_goal = FollowJointTrajectoryGoal()
trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
@ -75,25 +75,25 @@ class MultiPointCommand(hm.HelloNode):
# trajectory points
trajectory_goal.trajectory.points = [point0, point1, point2, point3, point4, point5]
# Specify the coordinate frame that we want (base_link) and set the time to be now.
# Specify the coordinate frame that we want (base_link) and set the time to be now
trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
trajectory_goal.trajectory.header.frame_id = 'base_link'
# Make the action call and send the goal. The last line of code waits
# for the result before it exits the python script.
# for the result before it exits the python script
self.trajectory_client.send_goal(trajectory_goal)
rospy.loginfo('Sent list of goals = {0}'.format(trajectory_goal))
self.trajectory_client.wait_for_result()
# Create a funcion, main(), to do all of the setup the hm.HelloNode class
# and issue the stow command.
# and issue the stow command
def main(self):
"""
Function that initiates the multipoint_command function.
:param self: The self reference.
"""
# The arguments of the main function of the hm.HelloNode class are the
# node_name, node topic namespace, and boolean (default value is true).
# node_name, node topic namespace, and boolean (default value is true)
hm.HelloNode.main(self, 'multipoint_command', 'multipoint_command', wait_for_first_pointcloud=False)
rospy.loginfo('issuing multipoint command...')
self.issue_multipoint_command()
@ -102,8 +102,7 @@ class MultiPointCommand(hm.HelloNode):
if __name__ == '__main__':
try:
# Initialize the MultiPointCommand() class and set it to node and run the
# main() function.
# Instantiate the MultiPointCommand class and execute the main() method
node = MultiPointCommand()
node.main()
except KeyboardInterrupt:

+ 15
- 12
src/scan_filter.py View File

@ -4,10 +4,10 @@
import rospy
from numpy import linspace, inf
from math import sin
# We're going to subscribe to a LaserScan message.
# We're going to subscribe to a LaserScan message
from sensor_msgs.msg import LaserScan
class Scanfilter:
class ScanFilter:
"""
A class that implements a LaserScan filter that removes all of the points.
that are not infront of the robot.
@ -15,19 +15,22 @@ class Scanfilter:
def __init__(self):
# We're going to assume that the robot is pointing up the x-axis, so that
# any points with y coordinates further than half of the defined
# width (1 meter) from the axis are not considered.
# width (1 meter) from the axis are not considered
self.width = 1
self.extent = self.width / 2.0
# Set up a subscriber. We're going to subscribe to the topic "scan",
# looking for LaserScan messages. When a message comes in, ROS is going
# to pass it to the function "callback" automatically.
# to pass it to the function "callback" automatically
self.sub = rospy.Subscriber('/scan', LaserScan, self.callback)
# Set up a publisher. This will publish on a topic called "filtered_scan",
# with a LaserScan message type.
# with a LaserScan message type
self.pub = rospy.Publisher('filtered_scan', LaserScan, queue_size=10)
# Create log message
rospy.loginfo("Publishing the filtered_scan topic. Use RViz to visualize.")
def callback(self,msg):
"""
@ -38,27 +41,27 @@ class Scanfilter:
:publishes msg: updated laserscan message.
"""
# Figure out the angles of the scan. We're going to do this each time, in case we're subscribing to more than one
# laser, with different numbers of beams.
# laser, with different numbers of beams
angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
# Work out the y coordinates of the ranges.
# Work out the y coordinates of the ranges
points = [r * sin(theta) if (theta < -2.5 or theta > 2.5) else inf for r,theta in zip(msg.ranges, angles)]
# If we're close to the x axis, keep the range, otherwise use inf, which means "no return".
# If we're close to the x axis, keep the range, otherwise use inf, which means "no return"
new_ranges = [r if abs(y) < self.extent else inf for r,y in zip(msg.ranges, points)]
# Substitute in the new ranges in the original message, and republish it.
# Substitute in the new ranges in the original message, and republish it
msg.ranges = new_ranges
self.pub.publish(msg)
if __name__ == '__main__':
# Initialize the node, and call it "scan_filter".
# Initialize the node, and call it "scan_filter"
rospy.init_node('scan_filter')
# Setup Scanfilter class
# Instantiate the ScanFilter class
Scanfilter()
# Give control to ROS. This will allow the callback to be called whenever new
# messages come in. If we don't put this line in, then the node will not work,
# and ROS will not process any messages.
# and ROS will not process any messages
rospy.spin()

+ 8
- 8
src/single_joint_actuator.py View File

@ -12,14 +12,14 @@ from control_msgs.msg import FollowJointTrajectoryGoal
# robot trajectories.
from trajectory_msgs.msg import JointTrajectoryPoint
# Import hello_misc script for handling trajecotry goals with an action client.
# Import hello_misc script for handling trajecotry goals with an action client
import hello_helpers.hello_misc as hm
class SingleJointActuator(hm.HelloNode):
"""
A class that sends multiple joint trajectory goals to a single joint.
"""
# Initialize the inhereted hm.Hellonode class.
# Initialize the inhereted hm.Hellonode class
def __init__(self):
hm.HelloNode.__init__(self)
@ -40,12 +40,12 @@ class SingleJointActuator(hm.HelloNode):
########################################################################
# Set trajectory_goal as a FollowJointTrajectoryGoal and define
# the joint name.
# the joint name
trajectory_goal = FollowJointTrajectoryGoal()
trajectory_goal.trajectory.joint_names = ['joint_head_pan']
# Provide desired positions for joint name.
# Set positions for the following 5 trajectory points.
# Set positions for the following 5 trajectory points
point0 = JointTrajectoryPoint()
point0.positions = [0.65]
@ -61,7 +61,7 @@ class SingleJointActuator(hm.HelloNode):
trajectory_goal.trajectory.header.frame_id = 'base_link'
# Make the action call and send the goal. The last line of code waits
# for the result before it exits the python script.
# for the result before it exits the python script
self.trajectory_client.send_goal(trajectory_goal)
rospy.loginfo('Sent goal = {0}'.format(trajectory_goal))
self.trajectory_client.wait_for_result()
@ -72,7 +72,7 @@ class SingleJointActuator(hm.HelloNode):
:param self: The self reference.
"""
# The arguments of the main function of the hm.HelloNode class are the
# node_name, node topic namespace, and boolean (default value is true).
# node_name, node topic namespace, and boolean (default value is true)
hm.HelloNode.main(self, 'issue_command', 'issue_command', wait_for_first_pointcloud=False)
rospy.loginfo('issuing command...')
self.issue_command()
@ -81,8 +81,8 @@ class SingleJointActuator(hm.HelloNode):
if __name__ == '__main__':
try:
# Initialize the SingleJointActuator() class and set it to node and run the
# main() function.
# Declare object from the SingleJointActuator class. Then execute the
# main() method/function
node = SingleJointActuator()
node.main()
except KeyboardInterrupt:

+ 1
- 1
src/stored_data_plotter.py View File

@ -24,7 +24,7 @@ class Plotter():
####################### Copy the file name here! #######################
file_name = '2022-06-30_11:26:20-AM'
# Complete name of directory to pull from.
# Complete name of directory to pull from
self.completeName = os.path.join(dir_path, file_name)
# Store dataframe

+ 12
- 13
src/stow_command.py View File

@ -5,14 +5,14 @@ import rospy
import time
# Import the FollowJointTrajectoryGoal from the control_msgs.msg package to
# control the Stretch robot.
# control the Stretch robot
from control_msgs.msg import FollowJointTrajectoryGoal
# Import JointTrajectoryPoint from the trajectory_msgs package to define
# robot trajectories.
# robot trajectories
from trajectory_msgs.msg import JointTrajectoryPoint
# Import hello_misc script for handling trajecotry goals with an action client.
# Import hello_misc script for handling trajecotry goals with an action client
import hello_helpers.hello_misc as hm
class StowCommand(hm.HelloNode):
@ -20,7 +20,7 @@ class StowCommand(hm.HelloNode):
A class that sends a joint trajectory goal to stow the Stretch's arm.
'''
# Initialize the inhereted hm.Hellonode class.
# Initialize the inhereted hm.Hellonode class
def __init__(self):
hm.HelloNode.__init__(self)
@ -30,16 +30,16 @@ class StowCommand(hm.HelloNode):
Function that makes an action call and sends stow postion goal.
:param self: The self reference.
'''
# Set stow_point as a JointTrajectoryPoint().
# Set stow_point as a JointTrajectoryPoint()
stow_point = JointTrajectoryPoint()
stow_point.time_from_start = rospy.Duration(0.000)
# Provide desired positions of lift, wrist extension, and yaw of
# the wrist (in meters).
# the wrist (in meters)
stow_point.positions = [0.2, 0.0, 3.4]
# Set trajectory_goal as a FollowJointTrajectoryGoal and define
# the joint names as a list.
# the joint names as a list
trajectory_goal = FollowJointTrajectoryGoal()
trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
@ -47,25 +47,25 @@ class StowCommand(hm.HelloNode):
# set in stow_point
trajectory_goal.trajectory.points = [stow_point]
# Specify the coordinate frame that we want (base_link) and set the time to be now.
# Specify the coordinate frame that we want (base_link) and set the time to be now
trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
trajectory_goal.trajectory.header.frame_id = 'base_link'
# Make the action call and send the goal. The last line of code waits
# for the result before it exits the python script.
# for the result before it exits the python script
self.trajectory_client.send_goal(trajectory_goal)
rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal))
self.trajectory_client.wait_for_result()
# Create a funcion, main(), to do all of the setup the hm.HelloNode class
# and issue the stow command.
# and issue the stow command
def main(self):
'''
Function that initiates stow_command function.
:param self: The self reference.
'''
# The arguments of the main function of the hm.HelloNode class are the
# node_name, node topic namespace, and boolean (default value is true).
# node_name, node topic namespace, and boolean (default value is true)
hm.HelloNode.main(self, 'stow_command', 'stow_command', wait_for_first_pointcloud=False)
rospy.loginfo('stowing...')
self.issue_stow_command()
@ -74,8 +74,7 @@ class StowCommand(hm.HelloNode):
if __name__ == '__main__':
try:
# Initialize the StowCommand() class and set it to node and run the
# main() function.
# Declare object from StowCommand class then execute the main() method
node = StowCommand()
node.main()
except KeyboardInterrupt:

Loading…
Cancel
Save