def publish_obstacles(obstacle_publisher, obstacle_list):
msg_object_array = obstacleArray()
msg_object_array.header.stamp = rospy.Time.now()
for current_obstacle in obstacle_list:
msg_object = obstacle()
msg_object.x.data = current_obstacle['x']
msg_object.y.data = current_obstacle['y']
msg_object.z.data = current_obstacle['z']
msg_object.width.data = current_obstacle['width']
msg_object.height.data = current_obstacle['height']
msg_object.length.data = current_obstacle['length'] # should change to length
msg_object.classification.data = current_obstacle['classification']
msg_object_array.data.append(msg_object)
obstacle_publisher.publish(msg_object_array)
def publish_obstacles(self, obstacle_list):
print("Detected {} obstacles".format(len(obstacle_list)))
msg_object_array = wizzybug_msgs.obstacleArray()
msg_object_array.header.stamp = rospy.Time.now()
for obstacle in obstacle_list:
msg_object = wizzybug_msgs.obstacle()
msg_object.x.data = obstacle['x']
msg_object.y.data = obstacle['y']
msg_object.z.data = obstacle['z']
msg_object.width.data = obstacle['width']
msg_object.height.data = obstacle['height']
msg_object.length.data = obstacle[
'length'] # should change to length
msg_object.classification.data = obstacle['classification']
msg_object_array.data.append(msg_object)
self.obstacle_list_pub.publish(msg_object_array)
def objects_sub_callback(self, data):
lidar_obj = obstacle()
lidar_obj.y = 0.0
lidar_obj.x = self.lidar_dist
lidar_obj.width = wizzy_width
lidar_obj.length = wizzy_length
self.objects = data.data
if __name__ == '__main__':
rospy.init_node('ttc_node')
#
ttc_msg = ttc()
lidar_obj = obstacle()
obs_msg = obstacleArray()
#
inputs_container = CallbackItems()
obj_sub = rospy.Subscriber('wizzy/obstacle_list',
obstacleArray,
inputs_container.objects_sub_callback,
queue_size=1)
lidar_dist_to_obstacle_subscriber = rospy.Subscriber(
'/wizzy/lidar_proc',
lidar_data,
inputs_container.lidar_dist_to_obstacle_callback,
queue_size=1)
joy_sub = rospy.Subscriber('/cmd_vel',
Twist,
import rospy
import matplotlib
from wizzybug_msgs.msg import obstacle, obstacleArray
def obj1_creator():
obj1 = obstacle()
obj1.depth = 0.5
obj1.height = 1.0
obj1.width = 1.0
obj1.x = 3
obj1.y = 1
obj1.yaw = 0.3
return obj1
if __name__ == '__main__':
rospy.init_node('objects_publisher_debug')
obj_pub = rospy.Publisher('wizzy/obstacle_list', obstacleArray)
obj_array = obstacleArray()
obj_array.data.append(obj1_creator())
obj_pub.publish(obj_array)