Merge pull request #20 from usdot-fhwa-stol/feature/carma_to_carla_ac…

…kermann_cmd

Create carma_to_carla_ackermann_cmd

carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carla_to_autoware_vehicle_status

@@ -0,0 +1,100 @@
+#!/usr/bin/env python
+
+# Copyright (C) 2021 LEIDOS.
+#
+# Licensed under the Apache License, Version 2.0 (the "License"); you may not
+# use this file except in compliance with the License. You may obtain a copy of
+# the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+# License for the specific language governing permissions and limitations under
+# the License.
+
+"""
+Subscribe from CARLA :carla_msgs::CarlaEgoVehicleStatus
+    Topic: /carla/{}/vehicle_status
+
+Subscribe from CARLA :carla_msgs::CarlaEgoVehicleInfo
+    Topic: /carla/{}/vehicle_info
+
+Publish to CARMA :autoware_msgs::VehicleStatus
+    Topic: /hardware_interface/vehicle_status
+
+"""
+import math
+import rospy
+from carla_msgs.msg import CarlaEgoVehicleInfo
+from carla_msgs.msg import CarlaEgoVehicleStatus
+from autoware_msgs.msg import VehicleStatus, Gear
+
+pub = rospy.Publisher('/hardware_interface/vehicle_status', VehicleStatus, queue_size=1)
+vehicle_info = None
+
+max_steering_angle = rospy.get_param('/max_steering_angle', 70)
+
+def vehicle_info_callback(vehicle_info_msg):
+    """
+    callback for vehicle info
+    vehicle_info_msg type:
+        carla_msgs::CarlaEgoVehicleInfo
+    """
+    global vehicle_info
+    vehicle_info = vehicle_info_msg
+
+def vehicle_status_callback(vehicle_status_msg):
+    """
+    callback for vehicle status
+    vehicle_status_msg type:
+        carla_msgs::CarlaEgoVehicleStatus
+    """
+
+    if vehicle_info is None:
+        return
+
+    status = VehicleStatus()
+    status.header = vehicle_status_msg.header
+
+    # calculate max steering angle
+    global max_steering_angle
+    max_steering_angle = math.radians(max_steering_angle)
+
+    # get max steering angle (use smallest non-zero value of all wheels)
+    for wheel in vehicle_info.wheels:
+        if wheel.max_steer_angle and wheel.max_steer_angle < max_steering_angle:
+            max_steering_angle = wheel.max_steer_angle
+
+    status.angle = vehicle_status_msg.control.steer * math.degrees(max_steering_angle)
+    status.speed = vehicle_status_msg.velocity
+
+    if vehicle_status_msg.control.reverse:
+        status.gearshift = Gear.REVERSE
+    else:
+        status.gearshift = Gear.DRIVE
+
+    if vehicle_status_msg.control.manual_gear_shift:
+        status.drivemode = VehicleStatus.MODE_MANUAL
+    else:
+        status.drivemode = VehicleStatus.MODE_AUTO
+    pub.publish(status)
+
+
+def convert_status_carla_to_autoware():
+    """
+    main loop
+    """
+    rospy.init_node('convert_status_carla_to_autoware', anonymous=True)
+    role_name = rospy.get_param('/role_name', 'ego_vehicle')
+    rospy.Subscriber('/carla/{}/vehicle_status'.format(role_name), CarlaEgoVehicleStatus,
+                     vehicle_status_callback)
+    rospy.Subscriber('/carla/{}/vehicle_info'.format(role_name), CarlaEgoVehicleInfo,
+                     vehicle_info_callback)
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    print("carla_to_autoware_vehicle_status")
+    convert_status_carla_to_autoware()

carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_ackermann_cmd

@@ -0,0 +1,110 @@
+#!/usr/bin/env python
+
+# Copyright (C) 2021 LEIDOS.
+#
+# Licensed under the Apache License, Version 2.0 (the "License"); you may not
+# use this file except in compliance with the License. You may obtain a copy of
+# the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+# License for the specific language governing permissions and limitations under
+# the License.
+
+"""
+Subscribe from CARMA :autoware_msgs::VehicleCmd
+    Topic: /hardware_interface/vehicle_cmd
+
+Subscribe from CARMA :std_msgs::Float32
+    Topic: /guidance/twist_filter/result/twist/lateral_jerk
+
+Subscribe from CARMA :cav_msgs::GuidanceState
+    Topic: /guidance/state
+
+Publish to CARLA :ackermann_msgs::AckermannDrive
+    Topic: /carla/{}/ackermann_cmd
+
+"""
+import rospy
+from ackermann_msgs.msg import AckermannDrive
+from autoware_msgs.msg import VehicleCmd
+from cav_msgs.msg import GuidanceState
+from std_msgs.msg import Float32
+
+ackermann_cmd_pub = None
+guidance_state = GuidanceState.STARTUP
+init_status = True
+init_cmd = AckermannDrive()
+lateral_jerk = 0
+
+def guidance_state_callback(guidance_state_msg):
+    """
+    callback for guidance state subscribing from CARMA
+    guidance_state_msg type:
+        cav_msgs::GuidanceState
+    """
+    global init_status
+    global init_cmd
+    global guidance_state
+    guidance_state = guidance_state_msg.state
+    if guidance_state == GuidanceState.ENGAGED and init_status:
+        init_status = False
+        ## providing initial ackermann command to carla virtual vehicle
+        init_cmd.speed = rospy.get_param('/init_speed', 10)
+        init_cmd.acceleration = rospy.get_param('/init_acceleration', 1)
+        init_cmd.steering_angle = rospy.get_param('/init_steering_angle', 0)
+        init_cmd.jerk = rospy.get_param('/init_jerk', 0)
+        ackermann_cmd_pub.publish(init_cmd)
+
+def commanded_jerk_callback(lateral_jerk_msg):
+    """
+    callback for commanded jerk from CARMA
+    lateral_jerk_msg type:
+        std_msgs::Float32
+    """
+
+    global lateral_jerk
+    lateral_jerk = lateral_jerk_msg
+
+def vehicle_cmd_callback(vehicle_cmd):
+    """
+    callback for vehicle cmds subscribing from CARMA
+    vehicle_cmd type:
+        autoware_msgs::VehicleCmd
+    """
+
+    if guidance_state != GuidanceState.ENGAGED:
+        return
+    else:
+        ackermann_drive = AckermannDrive()
+        ackermann_drive.speed = vehicle_cmd.ctrl_cmd.linear_velocity
+        ackermann_drive.acceleration = vehicle_cmd.ctrl_cmd.linear_acceleration
+        ackermann_drive.steering_angle = vehicle_cmd.ctrl_cmd.steering_angle
+        ackermann_drive.jerk = lateral_jerk
+
+        ## publishing ackermann drive msg to CARLA virtual vehicle
+        ackermann_cmd_pub.publish(ackermann_drive)
+
+
+def vehicle_cmd_to_ackermanndrive():
+    """
+    mainloop
+    """
+    global ackermann_cmd_pub
+    rospy.init_node('carma_to_carla_vehicle_cmd')
+    role_name = rospy.get_param('/role_name', 'ego_vehicle')
+    # pub init
+    ackermann_cmd_pub = rospy.Publisher('/carla/{}/ackermann_cmd'.format(role_name), AckermannDrive, queue_size=1)
+    # sub init
+    rospy.Subscriber('/hardware_interface/vehicle_cmd', VehicleCmd, vehicle_cmd_callback, queue_size=1)
+    rospy.Subscriber('/guidance/state', GuidanceState, guidance_state_callback, queue_size=1)
+    rospy.Subscriber('/guidance/twist_filter/result/twist/lateral_jerk', Float32, commanded_jerk_callback, queue_size=1)
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    print("carma_to_carla_vehicle_cmd")
+    vehicle_cmd_to_ackermanndrive()