These are sample solutions for the Python scripts from Coding Exercise #1 from Class Meeting 02.
point_location_sender.py
#!/usr/bin/env python3
""" This script publishes ROS messages containing the 3D coordinates of a single point """
import rospy
from geometry_msgs.msg import Point, PointStamped
from std_msgs.msg import Header
rospy.init_node('send_point_location')
my_header = Header(stamp=rospy.Time.now(), frame_id="odom")
my_point = Point(1.0, 2.0, 0.0)
my_point_stamped = PointStamped(header=my_header, point=my_point)
publisher = rospy.Publisher('/my_point', PointStamped, queue_size=10)
r = rospy.Rate(2)
while not rospy.is_shutdown():
my_point_stamped.header.stamp = rospy.Time.now()
publisher.publish(my_point_stamped)
r.sleep()
point_location_receiver.py
#!/usr/bin/env python3
""" This script receives ROS messages containing the 3D coordinates of a single point and prints them out """
import rospy
from geometry_msgs.msg import PointStamped
rospy.init_node('receive_point_location')
def process_point(data):
print("[{:.2f}, {:.2f}, {:.2f}]".format(data.point.x, data.point.y, data.point.z))
rospy.Subscriber("/my_point", PointStamped, process_point)
rospy.spin()
point_location_sender.py (object-oriented)
#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import Point, PointStamped
from std_msgs.msg import Header
class SendPointLocation(object):
""" This node publishes ROS messages containing the 3D coordinates of a single point """
def __init__(self):
rospy.init_node('send_point_location')
self.point_location_pub = rospy.Publisher('/my_point', PointStamped, queue_size=10)
def run(self):
my_header = Header(stamp=rospy.Time.now(), frame_id="odom")
my_point = Point(1.0, 2.0, 0.0)
my_point_stamped = PointStamped(header=my_header, point=my_point)
r = rospy.Rate(2)
while not rospy.is_shutdown():
my_point_stamped.header.stamp = rospy.Time.now()
self.point_location_pub.publish(my_point_stamped)
r.sleep()
if __name__ == '__main__':
node = SendPointLocation()
node.run()
point_location_receiver.py (object-oriented)
#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import PointStamped
class ReceivePointLocation(object):
""" This node receives ROS messages containing the 3D coordinates of a single point and prints them out """
def __init__(self):
rospy.init_node('receive_point_location')
rospy.Subscriber("/my_point", PointStamped, self.process_point)
def process_point(self, data):
print("[{:.2f}, {:.2f}, {:.2f}]".format(data.point.x, data.point.y, data.point.z))
def run(self):
rospy.spin()
if __name__ == '__main__':
node = ReceivePointLocation()
node.run()
stop_at_wall.py
#!/usr/bin/env python3
# TOPICS:
# cmd_vel: publish to, used for setting robot velocity
# scan : subscribing, where the wall is
import rospy
# msg needed for /scan.
from sensor_msgs.msg import LaserScan
# msgs needed for /cmd_vel.
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Vector3
# How close we will get to wall.
distance = 0.4
class StopAtWall(object):
""" This node walks the robot to wall and stops """
def __init__(self):
# Start rospy node.
rospy.init_node("walk_to_wall")
# Declare our node as a subscriber to the scan topic and
# set self.process_scan as the function to be used for callback.
rospy.Subscriber("/scan", LaserScan, self.process_scan)
# Get a publisher to the cmd_vel topic.
self.twist_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
# Create a default twist msg (all values 0).
lin = Vector3()
ang = Vector3()
self.twist = Twist(linear=lin,angular=ang)
def process_scan(self, data):
# Determine closeness to wall by looking at scan data from in front of
# the robot, set linear velocity based on that information, and
# publish to cmd_vel.
# The ranges field is a list of 360 number where each number
# corresponds to the distance to the closest obstacle from the
# LiDAR at various angles. Each measurement is 1 degree apart.
# The first entry in the ranges list corresponds with what's directly
# in front of the robot.
if data.ranges[0] >= distance:
# Go forward if not close enough to wall.
self.twist.linear.x = 0.1
else:
# Close enough to wall, stop.
self.twist.linear.x = 0
# Publish msg to cmd_vel.
self.twist_pub.publish(self.twist)
def run(self):
# Keep the program alive.
rospy.spin()
if __name__ == '__main__':
# Declare a node and run it.
node = StopAtWall()
node.run()