Browse Source

Fixed typos.

main
Alan G. Sanchez 2 years ago
parent
commit
ec6bb6a88b
1 changed files with 9 additions and 9 deletions
  1. +9
    -9
      src/aruco_tag_locator.py

+ 9
- 9
src/aruco_tag_locator.py View File

@ -48,16 +48,16 @@ class LocateArUcoTag(hm.HelloNode):
self.joint_state = None self.joint_state = None
# Provide the min and max joint positions for the head pan. These values # Provide the min and max joint positions for the head pan. These values
# are needed for sweeping the head to search for the ArUco tag.
# are needed for sweeping the head to search for the ArUco tag
self.min_pan_position = -4.10 self.min_pan_position = -4.10
self.max_pan_position = 1.50 self.max_pan_position = 1.50
# Define the number of steps for the sweep, then create the step size for # Define the number of steps for the sweep, then create the step size for
# the head pan joint.
# the head pan joint
self.pan_num_steps = 10 self.pan_num_steps = 10
self.pan_step_size = abs(self.min_pan_position - self.max_pan_position)/self.pan_num_steps self.pan_step_size = abs(self.min_pan_position - self.max_pan_position)/self.pan_num_steps
# Define the min tilt position, number of steps, and step size.
# Define the min tilt position, number of steps, and step size
self.min_tilt_position = -0.75 self.min_tilt_position = -0.75
self.tilt_num_steps = 3 self.tilt_num_steps = 3
self.tilt_step_size = pi/16 self.tilt_step_size = pi/16
@ -74,8 +74,8 @@ class LocateArUcoTag(hm.HelloNode):
def send_command(self, command): def send_command(self, command):
''' '''
Handles single joint control commands by constructing a FollowJointTrajectoryGoal message and sending it to the trajectory_client
created in hello_misc.
Handles single joint control commands by constructing a FollowJointTrajectoryGoal
message and sending it to the trajectory_client created in hello_misc.
:param self: The self reference. :param self: The self reference.
:param command: A dictionary message type. :param command: A dictionary message type.
''' '''
@ -147,7 +147,7 @@ class LocateArUcoTag(hm.HelloNode):
# Use a try-except block # Use a try-except block
try: try:
# Look up transform between the base_link and located ArUco tag
# Look up transform between the base_link and requested ArUco tag
transform = self.tf_buffer.lookup_transform('base_link', transform = self.tf_buffer.lookup_transform('base_link',
tag_name, tag_name,
rospy.Time()) rospy.Time())
@ -182,7 +182,7 @@ class LocateArUcoTag(hm.HelloNode):
# Create a StaticTranformBoradcaster Node. Also, start a Tf buffer that # Create a StaticTranformBoradcaster Node. Also, start a Tf buffer that
# will store the tf information for a few seconds.Then set up a tf listener, which # will store the tf information for a few seconds.Then set up a tf listener, which
# will subscribe to all of the relevant tf topics, and keep track of the information.
# will subscribe to all of the relevant tf topics, and keep track of the information
self.static_broadcaster = tf2_ros.StaticTransformBroadcaster() self.static_broadcaster = tf2_ros.StaticTransformBroadcaster()
self.tf_buffer = tf2_ros.Buffer() self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer) self.listener = tf2_ros.TransformListener(self.tf_buffer)
@ -190,7 +190,7 @@ class LocateArUcoTag(hm.HelloNode):
# Give the listener some time to accumulate transforms # Give the listener some time to accumulate transforms
rospy.sleep(1.0) rospy.sleep(1.0)
# Notify Stretch is searchring for the ArUco tag with a rospy loginfo fucntion
# Notify Stretch is searching for the ArUco tag with a rospy loginfo function
rospy.loginfo('Searching for docking ArUco tag.') rospy.loginfo('Searching for docking ArUco tag.')
# Search for the ArUco marker for the docking station # Search for the ArUco marker for the docking station
@ -202,7 +202,7 @@ if __name__ == '__main__':
try: try:
# Instantiate the `LocateArUcoTag()` object # Instantiate the `LocateArUcoTag()` object
node = LocateArUcoTag() node = LocateArUcoTag()
# Run the `main()` method.
# Run the `main()` method
node.main() node.main()
except KeyboardInterrupt: except KeyboardInterrupt:
rospy.loginfo('interrupt received, so shutting down') rospy.loginfo('interrupt received, so shutting down')

Loading…
Cancel
Save