Browse Source

Included the proper python version.

main
Alan G. Sanchez 2 years ago
parent
commit
bb3e31d3d5
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

@ -15,11 +15,11 @@ import hello_helpers.hello_misc as hm
from sensor_msgs.msg import JointState
# 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 TransformStamped from the geometry_msgs package for the publisher
@ -38,7 +38,7 @@ class LocateArUcoTag(hm.HelloNode):
# Initialize the inhereted hm.Hellonode class
hm.HelloNode.__init__(self)
# Initialize Subscriber
# Initialize subscriber
self.joint_states_sub = rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback)
# Initialize publisher
@ -62,7 +62,7 @@ class LocateArUcoTag(hm.HelloNode):
self.tilt_num_steps = 3
self.tilt_step_size = pi/16
# Define the head actuation rotational velocity
# Define the head actuation rotational velocity
self.rot_vel = 0.5 # radians per sec
@ -110,7 +110,7 @@ class LocateArUcoTag(hm.HelloNode):
# extract the head position value from the `position` key
point.positions = [command['position']]
# Set the rotaitonal velocity
# Set the rotational velocity
point.velocities = [self.rot_vel]
# Assign goal position with updated point variable
@ -129,19 +129,19 @@ class LocateArUcoTag(hm.HelloNode):
def find_tag(self, tag_name='docking_station'):
"""
A function that actuates the camera to search for a defined ArUco tag
marker. Then the function returns the pose
marker. Then the function returns the pose.
:param self: The self reference.
:param tag_name: A string value of the ArUco marker name.
:returns transform: A TransformStamped message type.
"""
# Create a dictionaries to get the head in its initial position for its search
# Create dictionaries to get the head in its initial position
pan_command = {'joint': 'joint_head_pan', 'position': self.min_pan_position}
self.send_command(pan_command)
tilt_command = {'joint': 'joint_head_tilt', 'position': self.min_tilt_position}
self.send_command(tilt_command)
# Nested for loop to sweep the pan and tilt in increments
# Nested for loop to sweep the joint_head_pan and joint_head_tilt in increments
for i in range(self.tilt_num_steps):
for j in range(self.pan_num_steps):
# Update the joint_head_pan position by the pan_step_size
@ -197,7 +197,7 @@ class LocateArUcoTag(hm.HelloNode):
# Give the listener some time to accumulate transforms
rospy.sleep(1.0)
# Notify Stretch is searching for the ArUco tag with a rospy loginfo function
# Notify Stretch is searching for the ArUco tag with `rospy.loginfo()`
rospy.loginfo('Searching for docking ArUco tag.')
# Search for the ArUco marker for the docking station

Loading…
Cancel
Save