def send_vehicle_msgs(self):
"""
Function (override) to send odometry message of the ego vehicle
instead of an object message.
The ego vehicle doesn't send its information as part of the object list.
A nav_msgs.msg.Odometry is prepared to be published
:return:
"""
vehicle_status = CarlaEgoVehicleStatus()
vehicle_status.header.stamp = self.get_current_ros_time()
vehicle_status.velocity = self.get_vehicle_speed_abs(self.carla_actor)
vehicle_status.acceleration = self.get_vehicle_acceleration_abs(
self.carla_actor)
vehicle_status.orientation = self.get_current_ros_pose().orientation
vehicle_status.control.throttle = self.carla_actor.get_control(
).throttle
vehicle_status.control.steer = self.carla_actor.get_control().steer
vehicle_status.control.brake = self.carla_actor.get_control().brake
vehicle_status.control.hand_brake = self.carla_actor.get_control(
).hand_brake
vehicle_status.control.reverse = self.carla_actor.get_control().reverse
vehicle_status.control.gear = self.carla_actor.get_control().gear
vehicle_status.control.manual_gear_shift = self.carla_actor.get_control(
).manual_gear_shift
self.publish_ros_message(self.topic_name() + "/vehicle_status",
vehicle_status)
if not self.vehicle_info_published:
self.vehicle_info_published = True
vehicle_info = CarlaEgoVehicleInfo()
vehicle_info.id = self.carla_actor.id
vehicle_info.type = self.carla_actor.type_id
vehicle_info.rolename = self.carla_actor.attributes.get(
'role_name')
vehicle_physics = self.carla_actor.get_physics_control()
for wheel in vehicle_physics.wheels:
wheel_info = CarlaEgoVehicleInfoWheel()
wheel_info.tire_friction = wheel.tire_friction
wheel_info.damping_rate = wheel.damping_rate
wheel_info.steer_angle = math.radians(wheel.steer_angle)
wheel_info.disable_steering = wheel.disable_steering
vehicle_info.wheels.append(wheel_info)
vehicle_info.max_rpm = vehicle_physics.max_rpm
vehicle_info.max_rpm = vehicle_physics.max_rpm
vehicle_info.moi = vehicle_physics.moi
vehicle_info.damping_rate_full_throttle = vehicle_physics.damping_rate_full_throttle
vehicle_info.damping_rate_zero_throttle_clutch_engaged = \
vehicle_physics.damping_rate_zero_throttle_clutch_engaged
vehicle_info.damping_rate_zero_throttle_clutch_disengaged = \
vehicle_physics.damping_rate_zero_throttle_clutch_disengaged
vehicle_info.use_gear_autobox = vehicle_physics.use_gear_autobox
vehicle_info.gear_switch_time = vehicle_physics.gear_switch_time
vehicle_info.clutch_strength = vehicle_physics.clutch_strength
vehicle_info.mass = vehicle_physics.mass
vehicle_info.drag_coefficient = vehicle_physics.drag_coefficient
vehicle_info.center_of_mass.x = vehicle_physics.center_of_mass.x
vehicle_info.center_of_mass.y = vehicle_physics.center_of_mass.y
vehicle_info.center_of_mass.z = vehicle_physics.center_of_mass.z
self.publish_ros_message(self.topic_name() + "/vehicle_info",
vehicle_info, True)
# @todo: do we still need this?
odometry = Odometry(header=self.get_msg_header())
odometry.child_frame_id = self.get_frame_id()
odometry.pose.pose = self.get_current_ros_pose()
odometry.twist.twist = self.get_current_ros_twist()
self.publish_ros_message(self.topic_name() + "/odometry", odometry)
def send_vehicle_msgs(self):
"""
send messages related to vehicle status
:return:
"""
vehicle_status = CarlaEgoVehicleStatus(
header=self.get_msg_header("map"))
vehicle_status.velocity = self.get_vehicle_speed_abs(self.carla_actor)
vehicle_status.acceleration.linear = self.get_current_ros_accel(
).linear
vehicle_status.orientation = self.get_current_ros_pose().orientation
vehicle_status.control.throttle = self.carla_actor.get_control(
).throttle
vehicle_status.control.steer = self.carla_actor.get_control().steer
vehicle_status.control.brake = self.carla_actor.get_control().brake
vehicle_status.control.hand_brake = self.carla_actor.get_control(
).hand_brake
vehicle_status.control.reverse = self.carla_actor.get_control().reverse
vehicle_status.control.gear = self.carla_actor.get_control().gear
vehicle_status.control.manual_gear_shift = self.carla_actor.get_control(
).manual_gear_shift
self.vehicle_status_publisher.publish(vehicle_status)
# only send vehicle once (in latched-mode)
if not self.vehicle_info_published:
self.vehicle_info_published = True
vehicle_info = CarlaEgoVehicleInfo()
vehicle_info.id = self.carla_actor.id
vehicle_info.type = self.carla_actor.type_id
vehicle_info.rolename = self.carla_actor.attributes.get(
'role_name')
vehicle_physics = self.carla_actor.get_physics_control()
for wheel in vehicle_physics.wheels:
wheel_info = CarlaEgoVehicleInfoWheel()
wheel_info.tire_friction = wheel.tire_friction
wheel_info.damping_rate = wheel.damping_rate
wheel_info.max_steer_angle = math.radians(
wheel.max_steer_angle)
wheel_info.radius = wheel.radius
wheel_info.max_brake_torque = wheel.max_brake_torque
wheel_info.max_handbrake_torque = wheel.max_handbrake_torque
wheel_info.position.x = (wheel.position.x/100.0) - \
self.carla_actor.get_transform().location.x
wheel_info.position.y = -(
(wheel.position.y / 100.0) -
self.carla_actor.get_transform().location.y)
wheel_info.position.z = (wheel.position.z/100.0) - \
self.carla_actor.get_transform().location.z
vehicle_info.wheels.append(wheel_info)
vehicle_info.max_rpm = vehicle_physics.max_rpm
vehicle_info.max_rpm = vehicle_physics.max_rpm
vehicle_info.moi = vehicle_physics.moi
vehicle_info.damping_rate_full_throttle = vehicle_physics.damping_rate_full_throttle
vehicle_info.damping_rate_zero_throttle_clutch_engaged = \
vehicle_physics.damping_rate_zero_throttle_clutch_engaged
vehicle_info.damping_rate_zero_throttle_clutch_disengaged = \
vehicle_physics.damping_rate_zero_throttle_clutch_disengaged
vehicle_info.use_gear_autobox = vehicle_physics.use_gear_autobox
vehicle_info.gear_switch_time = vehicle_physics.gear_switch_time
vehicle_info.clutch_strength = vehicle_physics.clutch_strength
vehicle_info.mass = vehicle_physics.mass
vehicle_info.drag_coefficient = vehicle_physics.drag_coefficient
vehicle_info.center_of_mass.x = vehicle_physics.center_of_mass.x
vehicle_info.center_of_mass.y = vehicle_physics.center_of_mass.y
vehicle_info.center_of_mass.z = vehicle_physics.center_of_mass.z
self.vehicle_info_publisher.publish(vehicle_info)
def send_vehicle_msgs(self):
"""
send messages related to vehicle status
:return:
"""
vehicle_status = CarlaEgoVehicleStatus(
header=self.get_msg_header("map"))
vehicle_status.velocity = self.get_vehicle_speed_abs(self.carla_actor)
vehicle_status.acceleration.linear = transforms.carla_vector_to_ros_vector_rotated(
self.carla_actor.get_acceleration(),
self.carla_actor.get_transform().rotation)
vehicle_status.orientation = self.get_current_ros_pose().orientation
vehicle_status.control.throttle = self.carla_actor.get_control(
).throttle
vehicle_status.control.steer = self.carla_actor.get_control().steer
vehicle_status.control.brake = self.carla_actor.get_control().brake
vehicle_status.control.hand_brake = self.carla_actor.get_control(
).hand_brake
vehicle_status.control.reverse = self.carla_actor.get_control().reverse
vehicle_status.control.gear = self.carla_actor.get_control().gear
vehicle_status.control.manual_gear_shift = self.carla_actor.get_control(
).manual_gear_shift
self.publish_message(self.get_topic_prefix() + "/vehicle_status",
vehicle_status)
# only send vehicle once (in latched-mode)
if not self.vehicle_info_published:
self.vehicle_info_published = True
vehicle_info = CarlaEgoVehicleInfo()
vehicle_info.id = self.carla_actor.id
vehicle_info.type = self.carla_actor.type_id
vehicle_info.rolename = self.carla_actor.attributes.get(
'role_name')
vehicle_physics = self.carla_actor.get_physics_control()
for wheel in vehicle_physics.wheels:
wheel_info = CarlaEgoVehicleInfoWheel()
wheel_info.tire_friction = wheel.tire_friction
wheel_info.damping_rate = wheel.damping_rate
wheel_info.max_steer_angle = math.radians(
wheel.max_steer_angle)
vehicle_info.wheels.append(wheel_info)
vehicle_info.max_rpm = vehicle_physics.max_rpm
vehicle_info.max_rpm = vehicle_physics.max_rpm
vehicle_info.moi = vehicle_physics.moi
vehicle_info.damping_rate_full_throttle = vehicle_physics.damping_rate_full_throttle
vehicle_info.damping_rate_zero_throttle_clutch_engaged = \
vehicle_physics.damping_rate_zero_throttle_clutch_engaged
vehicle_info.damping_rate_zero_throttle_clutch_disengaged = \
vehicle_physics.damping_rate_zero_throttle_clutch_disengaged
vehicle_info.use_gear_autobox = vehicle_physics.use_gear_autobox
vehicle_info.gear_switch_time = vehicle_physics.gear_switch_time
vehicle_info.clutch_strength = vehicle_physics.clutch_strength
vehicle_info.mass = vehicle_physics.mass
vehicle_info.drag_coefficient = vehicle_physics.drag_coefficient
vehicle_info.center_of_mass.x = vehicle_physics.center_of_mass.x
vehicle_info.center_of_mass.y = vehicle_physics.center_of_mass.y
vehicle_info.center_of_mass.z = vehicle_physics.center_of_mass.z
self.publish_message(self.get_topic_prefix() + "/vehicle_info",
vehicle_info, True)
# @todo: do we still need this?
odometry = Odometry(header=self.get_msg_header("map"))
odometry.child_frame_id = self.get_prefix()
odometry.pose.pose = self.get_current_ros_pose()
odometry.twist.twist = self.get_current_ros_twist()
self.publish_message(self.get_topic_prefix() + "/odometry", odometry)