From 5b6646396b716046c16cfbf726210882c1af429c Mon Sep 17 00:00:00 2001 From: "Alan G. Sanchez" Date: Fri, 26 Aug 2022 17:18:27 -0700 Subject: [PATCH] Fixed typos. --- src/avoider.py | 10 ++++------ src/edge_detection.py | 2 +- src/effort_sensing.py | 16 +++++----------- src/joint_state_printer.py | 2 +- src/marker.py | 10 ++++------ src/move.py | 5 ++--- src/multipoint_command.py | 15 +++++++-------- src/pointcloud_transformer.py | 6 +++--- src/scan_filter.py | 5 ++--- src/single_joint_actuator.py | 15 ++++++++------- src/speech_text.py | 3 +-- src/stored_data_plotter.py | 6 ++---- src/stow_command.py | 16 +++++++--------- src/tf2_broadcaster.py | 6 +++--- src/tf2_listener.py | 8 +++----- src/voice_teleoperation_base.py | 17 +++++++---------- 16 files changed, 60 insertions(+), 82 deletions(-) diff --git a/src/avoider.py b/src/avoider.py index f73ee70..1a5379f 100755 --- a/src/avoider.py +++ b/src/avoider.py @@ -27,7 +27,7 @@ class Avoider: 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) - # We're going to assume that the robot is pointing up the x-axis, so that + # We're going to assume that the robot is pointing at 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. self.width = 1 @@ -47,8 +47,6 @@ 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. @@ -57,8 +55,8 @@ 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 + # 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)) # Work out the y coordinates of the ranges @@ -81,7 +79,7 @@ if __name__ == '__main__': # Initialize the node, and call it "avoider" rospy.init_node('avoider') - # Instantiate he Avoider class + # Instantiate the Avoider class Avoider() # Give control to ROS. This will allow the callback to be called whenever new diff --git a/src/edge_detection.py b/src/edge_detection.py index eb185f8..eaa72a8 100755 --- a/src/edge_detection.py +++ b/src/edge_detection.py @@ -25,7 +25,7 @@ class EdgeDetection: parameter values. :param self: The self reference. """ - # Initialize the CvBridge class + # Instantiate a CvBridge() object self.bridge = CvBridge() # Initialize subscriber diff --git a/src/effort_sensing.py b/src/effort_sensing.py index 56f85f2..84db3b9 100755 --- a/src/effort_sensing.py +++ b/src/effort_sensing.py @@ -8,11 +8,11 @@ import os import csv # 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 # We're going to subscribe to 64-bit integers, so we need to import the definition @@ -31,7 +31,7 @@ class JointActuatorEffortSensor(hm.HelloNode): """ def __init__(self, export_data=False): """ - Function that initializes the subscriber,and other features. + Function that initializes the subscriber and other features. :param self: The self reference. :param export_data: A boolean message type. """ @@ -53,7 +53,6 @@ class JointActuatorEffortSensor(hm.HelloNode): # Create boolean data type for conditional statements if you want to export effort data. self.export_data = export_data - def callback(self, msg): """ Callback function to update and store JointState messages. @@ -63,7 +62,6 @@ class JointActuatorEffortSensor(hm.HelloNode): # Store the joint messages for later use self.joint_states = msg - def issue_command(self): """ Function that makes an action call and sends joint trajectory goals @@ -76,11 +74,11 @@ class JointActuatorEffortSensor(hm.HelloNode): trajectory_goal = FollowJointTrajectoryGoal() trajectory_goal.trajectory.joint_names = self.joints - # Provide desired positions for joint name. + # Provide desired positionsfor joint name. point0 = JointTrajectoryPoint() point0.positions = [0.9] - # Set a list to the trajectory_goal.trajectory.points + # Set a list to the `trajectory_goal.trajectory.points` trajectory_goal.trajectory.points = [point0] # Specify the coordinate frame that we want (base_link) and set the time to be now. @@ -93,7 +91,6 @@ class JointActuatorEffortSensor(hm.HelloNode): rospy.loginfo('Sent position goal = {0}'.format(trajectory_goal)) self.trajectory_client.wait_for_result() - def feedback_callback(self,feedback): """ The feedback_callback function deals with the incoming feedback messages @@ -130,7 +127,6 @@ class JointActuatorEffortSensor(hm.HelloNode): else: self.joint_effort.append(current_effort) - def done_callback(self, status, result): """ The done_callback function will be called when the joint action is complete. @@ -157,7 +153,6 @@ class JointActuatorEffortSensor(hm.HelloNode): writer.writerow(self.joints) writer.writerows(self.joint_effort) - def main(self): """ Function that initiates the issue_command function. @@ -170,7 +165,6 @@ class JointActuatorEffortSensor(hm.HelloNode): self.issue_command() time.sleep(2) - if __name__ == '__main__': try: # Declare object, node, from JointActuatorEffortSensor class diff --git a/src/joint_state_printer.py b/src/joint_state_printer.py index 1292f32..583c9cb 100755 --- a/src/joint_state_printer.py +++ b/src/joint_state_printer.py @@ -64,7 +64,7 @@ if __name__ == '__main__': # Initialize the node. rospy.init_node('joint_state_printer', anonymous=True) - # Declare object from JointStatePublisher class + # Instantiate a `JointStatePublisher()` object JSP = JointStatePublisher() # Use the rospy.sleep() function to allow the class to initialize before diff --git a/src/marker.py b/src/marker.py index 7a8c720..c4b8e24 100755 --- a/src/marker.py +++ b/src/marker.py @@ -3,7 +3,7 @@ # Import modules import rospy -# Import the Marker message type from the visualization_msgs package. +# Import the Marker message type from the visualization_msgs package from visualization_msgs.msg import Marker class Balloon(): @@ -18,7 +18,7 @@ class 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. + # 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 @@ -61,10 +61,9 @@ class Balloon(): # Create log message rospy.loginfo("Publishing the balloon topic. Use RViz to visualize.") - def publish_marker(self): """ - Function that publishes the sphere marker + Function that publishes the sphere marker. :param self: The self reference. :publishes self.marker: Marker message. @@ -72,12 +71,11 @@ class Balloon(): # publisher the marker self.pub.publish(self.marker) - if __name__ == '__main__': # Initialize the node, as usual rospy.init_node('marker') - # Declare object from the Balloon class + # Instanstiate a `Balloon()` object balloon = Balloon() # Set a rate diff --git a/src/move.py b/src/move.py index 62f2cbe..5691193 100755 --- a/src/move.py +++ b/src/move.py @@ -15,11 +15,10 @@ class Move: Function that initializes the publisher. :param self: The self reference """ - # Setup a publisher that will send the velocity commands for the Stretch + # Setup a publisher that will send the velocity commands to Stretch # 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 @@ -51,7 +50,7 @@ if __name__ == '__main__': # Initialize the node, and call it "move" rospy.init_node('move') - # Declare object from Move class + # Instanstiate a `Move()` object base_motion = Move() # Rate allows us to control the (approximate) rate at which we publish things. diff --git a/src/multipoint_command.py b/src/multipoint_command.py index 58da750..e35db9a 100755 --- a/src/multipoint_command.py +++ b/src/multipoint_command.py @@ -19,9 +19,11 @@ 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): + """ + Function that initializes the inhereted hm.HelloNode class. + :param self: The self reference. + """ hm.HelloNode.__init__(self) def issue_multipoint_command(self): @@ -33,8 +35,8 @@ class MultiPointCommand(hm.HelloNode): # Set point0 as a JointTrajectoryPoint() point0 = JointTrajectoryPoint() - # Provide desired positions of lift, wrist extension, and yaw of - # the wrist (in meters) + # Provide desired positions of lift (m), wrist extension (m), and yaw of + # the wrist (radians) point0.positions = [0.2, 0.0, 3.4] # Provide desired velocity of the lift (m/s), wrist extension (m/s), @@ -85,8 +87,6 @@ class MultiPointCommand(hm.HelloNode): 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 def main(self): """ Function that initiates the multipoint_command function. @@ -99,10 +99,9 @@ class MultiPointCommand(hm.HelloNode): self.issue_multipoint_command() time.sleep(2) - if __name__ == '__main__': try: - # Instantiate the MultiPointCommand class and execute the main() method + # Instanstiate a `MultiPointCommand()` object and execute the main() method node = MultiPointCommand() node.main() except KeyboardInterrupt: diff --git a/src/pointcloud_transformer.py b/src/pointcloud_transformer.py index aff0368..bf1e2a8 100755 --- a/src/pointcloud_transformer.py +++ b/src/pointcloud_transformer.py @@ -48,7 +48,7 @@ class PointCloudTransformer: """ A function that extracts the points from the stored PointCloud2 message and appends those points to a PointCloud message. Then the function transforms - the PointCloud from its the header frame id, 'camera_color_optical_frame' + the PointCloud from its header frame id, 'camera_color_optical_frame' to the 'base_link' frame. :param self: The self reference. """ @@ -91,7 +91,7 @@ if __name__=="__main__": # Initialize the node, and call it "pointcloud_transformer" rospy.init_node('pointcloud_transformer',anonymous=True) - # Declare object, PCT, from the PointCloudTransformer class. + # Instanstiate a `PointCloudTransformer()` object PCT = PointCloudTransformer() # We're going to publish information at 1 Hz @@ -103,6 +103,6 @@ if __name__=="__main__": # Run while loop until the node is shutdown while not rospy.is_shutdown(): - # Run the pcl_transformer method + # Run the `pcl_transformer()` method PCT.pcl_transformer() rate.sleep() diff --git a/src/scan_filter.py b/src/scan_filter.py index 7120f80..b5ee192 100755 --- a/src/scan_filter.py +++ b/src/scan_filter.py @@ -31,7 +31,6 @@ class ScanFilter: # Create log message rospy.loginfo("Publishing the filtered_scan topic. Use RViz to visualize.") - def callback(self,msg): """ Callback function to deal with incoming laserscan messages. @@ -40,8 +39,8 @@ 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 + # 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)) # Work out the y coordinates of the ranges diff --git a/src/single_joint_actuator.py b/src/single_joint_actuator.py index 0219392..6d4015a 100755 --- a/src/single_joint_actuator.py +++ b/src/single_joint_actuator.py @@ -5,11 +5,11 @@ 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 trajectory goals with an action client @@ -19,8 +19,11 @@ class SingleJointActuator(hm.HelloNode): """ A class that sends multiple joint trajectory goals to a single joint. """ - # Initialize the inhereted hm.Hellonode class def __init__(self): + """ + Function that initializes the inhereted hm.HelloNode class. + :param self: The self reference. + """ hm.HelloNode.__init__(self) def issue_command(self): @@ -60,7 +63,7 @@ class SingleJointActuator(hm.HelloNode): # trajectory points trajectory_goal.trajectory.points = [point0]#, point1] - # 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' @@ -82,11 +85,9 @@ class SingleJointActuator(hm.HelloNode): self.issue_command() time.sleep(2) - if __name__ == '__main__': try: - # Declare object from the SingleJointActuator class. Then execute the - # main() method/function + # Instanstiate a `SingleJointActuator()` object and execute the main() method node = SingleJointActuator() node.main() except KeyboardInterrupt: diff --git a/src/speech_text.py b/src/speech_text.py index 5bc9061..bf31ec0 100755 --- a/src/speech_text.py +++ b/src/speech_text.py @@ -7,7 +7,6 @@ import os # Import SpeechRecognitionCandidates from the speech_recognition_msgs package from speech_recognition_msgs.msg import SpeechRecognitionCandidates - class SpeechText: """ A class that saves the interpreted speech from the ReSpeaker Microphone Array to a text file. @@ -15,6 +14,7 @@ class SpeechText: def __init__(self): """ Initialize subscriber and directory to save speech to text file. + :param self: The self reference. """ # Initialize subscriber self.sub = rospy.Subscriber("speech_to_text", SpeechRecognitionCandidates, self.callback) @@ -44,7 +44,6 @@ class SpeechText: file_object.write("\n") file_object.write(transcript) - if __name__ == '__main__': # Initialize the node and name it speech_text rospy.init_node('speech_text') diff --git a/src/stored_data_plotter.py b/src/stored_data_plotter.py index b414fbd..76e8fc2 100755 --- a/src/stored_data_plotter.py +++ b/src/stored_data_plotter.py @@ -65,7 +65,6 @@ class Plotter(): # Reset y_anim for the next joint effort animation del self.y_anim[:] - # Conditional statement for regular plotting (No animation) else: self.data[joint].plot(kind='line') @@ -73,18 +72,17 @@ class Plotter(): # plt.savefig(save_path, bbox_inches='tight') plt.show() - def plot_animate(self,i): """ Function that plots every increment of the dataframe. :param self: The self reference. :param i: index value. """ - # Append self.effort values for given joint. + # Append self.effort values for given joint self.y_anim.append(self.effort.values[i]) plt.plot(self.y_anim, color='blue') - if __name__ == '__main__': + # Instanstiate a `Plotter()` object and execute the `plot_data()` method viz = Plotter(animate=True) viz.plot_data() diff --git a/src/stow_command.py b/src/stow_command.py index a267c7e..6f76f01 100755 --- a/src/stow_command.py +++ b/src/stow_command.py @@ -19,12 +19,13 @@ class StowCommand(hm.HelloNode): ''' A class that sends a joint trajectory goal to stow the Stretch's arm. ''' - - # Initialize the inhereted hm.Hellonode class def __init__(self): + """ + Function that initializes the inhereted hm.HelloNode class. + :param self: The self reference. + """ hm.HelloNode.__init__(self) - def issue_stow_command(self): ''' Function that makes an action call and sends stow postion goal. @@ -34,8 +35,8 @@ class StowCommand(hm.HelloNode): 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) + # Provide desired positions of lift (meters), wrist extension (meters), and yaw of + # the wrist (radians) stow_point.positions = [0.2, 0.0, 3.4] # Set trajectory_goal as a FollowJointTrajectoryGoal and define @@ -57,8 +58,6 @@ class StowCommand(hm.HelloNode): 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 stow_command function. @@ -71,10 +70,9 @@ class StowCommand(hm.HelloNode): self.issue_stow_command() time.sleep(2) - if __name__ == '__main__': try: - # Declare object from StowCommand class then execute the main() method + # Instanstiate a `StowCommand()` object and execute the main() method node = StowCommand() node.main() except KeyboardInterrupt: diff --git a/src/tf2_broadcaster.py b/src/tf2_broadcaster.py index f0027ae..6b6e27b 100755 --- a/src/tf2_broadcaster.py +++ b/src/tf2_broadcaster.py @@ -3,10 +3,10 @@ # Import modules import rospy -# Because of transformations +# Import tf.transformations the change quaternion values to euler values import tf.transformations -# The TransformStamped message is imported to create a child frame +# The TransformStamped message is imported to broadcast a transform frame from geometry_msgs.msg import TransformStamped # Import StaticTransformBroadcaster to publish static transforms @@ -84,7 +84,7 @@ if __name__ == '__main__': # Initialize the node, and call it "tf2_broadcaster" rospy.init_node('tf2_broadcaster') - # Instantiate the FixedFrameBroadcaster class + # Instantiate the `FixedFrameBroadcaster()` class FixedFrameBroadcaster() # Give control to ROS. This will allow the callback to be called whenever new diff --git a/src/tf2_listener.py b/src/tf2_listener.py index 51aac14..6a88ec1 100755 --- a/src/tf2_listener.py +++ b/src/tf2_listener.py @@ -9,11 +9,10 @@ from geometry_msgs.msg import TransformStamped # Import StaticTransformBroadcaster to publish static transforms import tf2_ros - class FrameListener(): """ - This Class prints the transformation between the fk_link_mast frame and the - target frame, link_grasp_center. + This Class prints the transform between the fk_link_mast and the + link_grasp_center frame. """ def __init__(self): """ @@ -55,12 +54,11 @@ class FrameListener(): # Manage the rate the node prints out a message rate.sleep() - if __name__ == '__main__': # Initialize the node, and call it "tf2_liistener" rospy.init_node('tf2_listener') - # Instantiate the FrameListener class + # Instantiate the `FrameListener()` class FrameListener() # Give control to ROS. This will allow the callback to be called whenever new diff --git a/src/voice_teleoperation_base.py b/src/voice_teleoperation_base.py index a86ac72..1262dbc 100755 --- a/src/voice_teleoperation_base.py +++ b/src/voice_teleoperation_base.py @@ -13,11 +13,11 @@ from sensor_msgs.msg import JointState from std_msgs.msg import Int32 # 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 trajectory goals with an action client @@ -141,11 +141,11 @@ class GetVoiceCommands: if self.voice_command == 'back': command = {'joint': 'translate_mobile_base', 'inc': -self.get_inc()['translate']} - # Move base left command + # Rotate base left command if self.voice_command == 'left': command = {'joint': 'rotate_mobile_base', 'inc': self.get_inc()['rad']} - # Move base right command + # Rotate base right command if self.voice_command == 'right': command = {'joint': 'rotate_mobile_base', 'inc': -self.get_inc()['rad']} @@ -222,20 +222,19 @@ class VoiceTeleopNode(hm.HelloNode): rospy.loginfo('inc = {0}'.format(inc)) new_value = inc - # Assign the new_value position to the trajectory goal message type. + # Assign the new_value position to the trajectory goal message type point.positions = [new_value] trajectory_goal.trajectory.points = [point] trajectory_goal.trajectory.header.stamp = rospy.Time.now() rospy.loginfo('joint_name = {0}, trajectory_goal = {1}'.format(joint_name, trajectory_goal)) - # Make the action call and send goal of the new joint position. + # Make the action call and send goal of the new joint position self.trajectory_client.send_goal(trajectory_goal) rospy.loginfo('Done sending command.') # Reprint the voice command menu self.speech.print_commands() - def main(self): """ The main function that instantiates the HelloNode class, initializes the subscriber, @@ -255,11 +254,9 @@ class VoiceTeleopNode(hm.HelloNode): self.send_command(command) rate.sleep() - if __name__ == '__main__': try: - # Declare object from the VoiceTeleopNode class. Then execute the - # main() method/function + # Instanstiate a `VoiceTeleopNode()` object and execute the main() method node = VoiceTeleopNode() node.main() except KeyboardInterrupt: