Init commit of src folder and nodes.

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()

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()

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()