def get_ros_msg(x, y, yaw, v, id):
quat = tf.transformations.quaternion_from_euler(0, 0, yaw)
m = Marker()
m.header.frame_id = "/map"
m.header.stamp = rospy.Time.now()
m.id = id
m.type = m.CUBE
m.pose.position.x = x + 1.3 * math.cos(yaw)
m.pose.position.y = y + 1.3 * math.sin(yaw)
m.pose.position.z = 0.75
m.pose.orientation = Quaternion(*quat)
m.scale.x = 4.475
m.scale.y = 1.850
m.scale.z = 1.645
m.color.r = 93 / 255.0
m.color.g = 122 / 255.0
m.color.b = 177 / 255.0
m.color.a = 0.97
o = Object()
o.header.frame_id = "/map"
o.header.stamp = rospy.Time.now()
o.id = id
o.classification = o.CLASSIFICATION_CAR
o.x = x
o.y = y
o.yaw = yaw
o.v = v
o.L = m.scale.x
o.W = m.scale.y
return {
"object_msg": o,
"marker_msg": m,
"quaternion": quat
}
def to_ros(self):
quat = tf.transformations.quaternion_from_euler(0, 0, self.yaw)
self.tf_broadcaster.sendTransform((self.x, self.y, 1.5), quat,
rospy.Time.now(),
"/car_" + str(self.id), "/map")
o = Object()
o.header.frame_id = "/map"
o.header.stamp = rospy.Time.now()
o.id = self.id
o.classification = o.CLASSIFICATION_CAR
o.x = self.x
o.y = self.y
o.yaw = self.yaw
o.v = self.v
# input u
o.a = self.accel
o.delta = self.delta
o.L = 4.475
o.W = 1.850
self.object_pub.publish(o)