simulated_range_sensors.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('cob_phidgets')
00003 import rospy
00004 import math
00005 from geometry_msgs.msg import *
00006 from sensor_msgs.msg import *
00007 
00008 class GazeboVirtualRangeSensor():
00009         
00010         def __init__(self, laser_topic, range_topic):           
00011                 rospy.Subscriber(laser_topic, LaserScan, self.laser_callback)
00012 #               rospy.logdebug("subscribed to laser topic: %s", laser_topic)
00013                 self.pub = rospy.Publisher(range_topic, Range)
00014 #               rospy.loginfo("topic '" + range_topic + "' advertised")
00015                 rospy.loginfo("adding sensor converting from topic '" + laser_topic + "' to topic '" + range_topic + "'")
00016 
00017         def laser_callback(self, msg):
00018                 sensor_range = Range()
00019         
00020                 # set header
00021                 sensor_range.header = msg.header
00022                 
00023                 # set range from average out of laser message
00024                 if len(msg.ranges) > 0:
00025                         sensor_range.range = sum(msg.ranges)/len(msg.ranges)
00026                 else:
00027                         sensor_range.range = 0
00028                 
00029                 # set min and max range
00030                 sensor_range.min_range = msg.range_min
00031                 sensor_range.max_range = msg.range_max
00032                 
00033                 # set field of view
00034                 sensor_range.field_of_view = msg.angle_max - msg.angle_min
00035                 
00036                 # set radiation type to IR (=1)
00037                 sensor_range.radiation_type = 1
00038                 
00039                 # publish range
00040                 self.pub.publish(sensor_range)
00041 
00042 if __name__ == "__main__":
00043         rospy.init_node('tactile_sensors')
00044         rospy.sleep(0.5)
00045         
00046         if not rospy.has_param('~sensors'):
00047                 rospy.logerr("no sensors specified")
00048                 exit(1)
00049 
00050         sensors = rospy.get_param('~sensors')
00051         
00052         if type(sensors) != list:
00053                 rospy.logerr("sensors parameter not a list")
00054                 exit(1)
00055 
00056         for sensor in sensors:
00057                 GazeboVirtualRangeSensor(sensor['laser_topic'], sensor['range_topic'])
00058 
00059         rospy.spin()


cob_phidgets
Author(s): Florian Weisshardt
autogenerated on Thu Aug 27 2015 12:46:06