From 6973e1a88dc438fbdc1421222050448a97be3e42 Mon Sep 17 00:00:00 2001 From: "Alan G. Sanchez" Date: Thu, 5 Aug 2021 17:10:37 -0700 Subject: [PATCH] Init commit of src folder and nodes. --- src/marker.py | 75 +++++++++++++++++++++++++++++++++++++++++++++++++++ src/move.py | 58 +++++++++++++++++++++++++++++++++++++++ src/scan.py | 27 +++++++++++++++++++ 3 files changed, 160 insertions(+) create mode 100755 src/marker.py create mode 100755 src/move.py create mode 100755 src/scan.py diff --git a/src/marker.py b/src/marker.py new file mode 100755 index 0000000..2f20a4d --- /dev/null +++ b/src/marker.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python + +# This node will publish a spherical marker above the current robot position, as +# it drives around the world. + +# Import rospy and sys, as usual +import rospy +import sys + +# Import the Marker message type from the visualization_msgs package. +from visualization_msgs.msg import Marker + +class Balloon(): + def __init__(self): + # Set up a publisher. We're going to publish on a topic called balloon. + self.publisher = 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. + self.marker.header.frame_id = '/base_link' + self.marker.header.stamp = rospy.Time() + self.marker.type = self.marker.SPHERE + + # 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. + self.marker.id = 0 + + # 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 + self.marker.scale.x = 0.5 + self.marker.scale.y = 0.5 + self.marker.scale.z = 0.5 + + # 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. + 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. + self.marker.pose.position.x = 0.0 + self.marker.pose.position.y = 0.0 + self.marker.pose.position.z = 2.0 + + def publish_marker(self): + # publisher the marker + self.publisher.publish(self.marker) + + +if __name__ == '__main__': + # Initialize the node, as usual + rospy.init_node('marker', argv=sys.argv) + + ballon = Balloon() + + # Set a rate. + rate = rospy.Rate(10) + + # Publish the marker at 10Hz. + while not rospy.is_shutdown(): + ballon.publish_marker() + rate.sleep() diff --git a/src/move.py b/src/move.py new file mode 100755 index 0000000..b6820a3 --- /dev/null +++ b/src/move.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python + +# Every python controller needs these lines +import rospy + +# The Twist message is used to send velocities to the robot. +from geometry_msgs.msg import Twist + +class Move: + def __init__(self): + # Setup a publisher that will send the velocity commands for the Stretch + # This will publish on a topic called "/stretch_diff_drive_controller/cmd_vel" + # with a message type Twist. + self.pub = rospy.Publisher('/stretch_diff_drive_controller/cmd_vel', Twist, queue_size=1) + + def move_forward(self): + # Make a Twist message. We're going to set all of the elements, since we + # 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. + 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. + command.angular.x = 0.0 + command.angular.y = 0.0 + command.angular.z = 0.0 + + # Publish the Twist commands + self.pub.publish(command) + +if __name__ == '__main__': + # Initialize the node, and call it "move". + rospy.init_node('move') + + # Setup Move class to base_motion + 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. + 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. + 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 + # 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. + rate.sleep() diff --git a/src/scan.py b/src/scan.py new file mode 100755 index 0000000..87b03e1 --- /dev/null +++ b/src/scan.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python + +# Every python controller needs these lines +import rospy + +# We're going to subscribe to a LaserScan message. +from sensor_msgs.msg import LaserScan + +class Scan: + def __init__(self): + # Set up a subscriber. We're going to subscribe to the topic "counter", + # looking for Int64 messages. When a message comes in, ROS is going to pass + # it to the function "callback" automatically. + self.sub = rospy.Subscriber('/scan', LaserScan, self.callback) + + def callback(self,msg): + print('Minimum scan value = ' + str(len(msg.ranges))) + rospy.sleep(1) + +if __name__ == '__main__': + # Initialize the node, and call it "move". + rospy.init_node('scan') + + # Setup Move class to base_motion + Scan() + + rospy.spin()