Browse Source

Fixed IndentiationError.

pull/14/head
Alan G. Sanchez 2 years ago
parent
commit
fe52d0956c
7 changed files with 96 additions and 90 deletions
  1. +10
    -9
      src/avoider.py
  2. +1
    -0
      src/joint_state_printer.py
  3. +1
    -0
      src/marker.py
  4. +1
    -0
      src/move.py
  5. +77
    -77
      src/multipoint_command.py
  6. +1
    -0
      src/scan_filter.py
  7. +5
    -4
      src/stow_command.py

+ 10
- 9
src/avoider.py View File

@ -18,9 +18,9 @@ class Avoider:
"""
def __init__(self):
"""
Function that initializes the subscriber, publisher, and marker features.
:param self: The self reference.
"""
Function that initializes the subscriber, publisher, and marker features.
:param self: The self reference.
"""
# 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.
@ -47,15 +47,16 @@ class Avoider:
self.twist.angular.y = 0.0
self.twist.angular.z = 0.0
# Called every time we get a LaserScan message from ROS.
def set_speed(self,msg):
"""
Callback function to deal with incoming laserscan messages.
:param self: The self reference.
:param msg: The subscribed laserscan message.
Callback function to deal with incoming laserscan messages.
:param self: The self reference.
:param msg: The subscribed laserscan message.
:publishes self.twist: Twist message.
"""
: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.
angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
@ -66,7 +67,7 @@ class Avoider:
# 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

+ 1
- 0
src/joint_state_printer.py View File

@ -21,6 +21,7 @@ class JointStatePublisher():
# Set up a subscriber. We're going to subscribe to the topic "joint_states"
self.sub = rospy.Subscriber('joint_states', JointState, self.callback)
def callback(self, msg):
"""
Callback function to deal with the incoming JointState messages.

+ 1
- 0
src/marker.py View File

@ -58,6 +58,7 @@ class Balloon():
self.marker.pose.position.y = 0.0
self.marker.pose.position.z = 2.0
def publish_marker(self):
"""
Function that publishes the sphere marker

+ 1
- 0
src/move.py View File

@ -19,6 +19,7 @@ class Move:
# 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
def move_forward(self):
"""
Function that publishes Twist messages

+ 77
- 77
src/multipoint_command.py View File

@ -20,83 +20,83 @@ class MultiPointCommand(hm.HelloNode):
A class that sends multiple joint trajectory goals to the stretch robot.
"""
# Initialize the inhereted hm.Hellonode class.
def __init__(self):
hm.HelloNode.__init__(self)
def issue_multipoint_command(self):
"""
Function that makes an action call and sends multiple joint trajectory goals.
:param self: The self reference.
"""
# Set point0 as a JointTrajectoryPoint().
point0 = JointTrajectoryPoint()
# Provide desired positions of lift, wrist extension, and yaw of
# 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).
# 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).
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.
point1 = JointTrajectoryPoint()
point1.positions = [0.3, 0.1, 2.0]
point2 = JointTrajectoryPoint()
point2.positions = [0.5, 0.2, -1.0]
point3 = JointTrajectoryPoint()
point3.positions = [0.6, 0.3, 0.0]
point4 = JointTrajectoryPoint()
point4.positions = [0.8, 0.2, 1.0]
point5 = JointTrajectoryPoint()
point5.positions = [0.5, 0.1, 0.0]
# Set trajectory_goal as a FollowJointTrajectoryGoal and define
# the joint names as a list.
trajectory_goal = FollowJointTrajectoryGoal()
trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
# Then trajectory_goal.trajectory.points is defined by a list of the joint
# 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.
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.
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.
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).
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)
# Initialize the inhereted hm.Hellonode class.
def __init__(self):
hm.HelloNode.__init__(self)
def issue_multipoint_command(self):
"""
Function that makes an action call and sends multiple joint trajectory goals.
:param self: The self reference.
"""
# Set point0 as a JointTrajectoryPoint().
point0 = JointTrajectoryPoint()
# Provide desired positions of lift, wrist extension, and yaw of
# 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).
# 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).
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.
point1 = JointTrajectoryPoint()
point1.positions = [0.3, 0.1, 2.0]
point2 = JointTrajectoryPoint()
point2.positions = [0.5, 0.2, -1.0]
point3 = JointTrajectoryPoint()
point3.positions = [0.6, 0.3, 0.0]
point4 = JointTrajectoryPoint()
point4.positions = [0.8, 0.2, 1.0]
point5 = JointTrajectoryPoint()
point5.positions = [0.5, 0.1, 0.0]
# Set trajectory_goal as a FollowJointTrajectoryGoal and define
# the joint names as a list.
trajectory_goal = FollowJointTrajectoryGoal()
trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
# Then trajectory_goal.trajectory.points is defined by a list of the joint
# 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.
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.
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.
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).
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__':

+ 1
- 0
src/scan_filter.py View File

@ -28,6 +28,7 @@ class Scanfilter:
# with a LaserScan message type.
self.pub = rospy.Publisher('filtered_scan', LaserScan, queue_size=10)
def callback(self,msg):
"""
Callback function to deal with incoming laserscan messages.

+ 5
- 4
src/stow_command.py View File

@ -17,18 +17,19 @@ import hello_helpers.hello_misc as hm
class StowCommand(hm.HelloNode):
'''
A class that sends a joint trajectory goal to stow the Stretch's arm.
'''
A class that sends a joint trajectory goal to stow the Stretch's arm.
'''
# Initialize the inhereted hm.Hellonode class.
def __init__(self):
hm.HelloNode.__init__(self)
def issue_stow_command(self):
'''
Function that makes an action call and sends stow postion goal.
Function that makes an action call and sends stow postion goal.
:param self: The self reference.
'''
'''
# Set stow_point as a JointTrajectoryPoint().
stow_point = JointTrajectoryPoint()
stow_point.time_from_start = rospy.Duration(0.000)

Loading…
Cancel
Save