diff --git a/CHANGELOG.md b/CHANGELOG.md index 9583ed44..79d65801 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,6 +7,10 @@ * Add node to convert a twist to a vehicle control command * Add node carla_spectator_camera * Update carla_waypoint_publisher +* Add carla_ros_scenario_runner +* Add rviz_carla_plugin +* Add carla_ad_agent +* Add carla_ad_demo ## CARLA-ROS-Bridge 0.9.6 diff --git a/README.md b/README.md index a075a57b..5f2dba4c 100644 --- a/README.md +++ b/README.md @@ -48,24 +48,18 @@ For more information about configuring a ROS environment see First run the simulator (see carla documentation: ) - ./CarlaUE4.sh -windowed -ResX=320 -ResY=240 + # run carla in background + SDL_VIDEODRIVER=offscreen ./CarlaUE4.sh -opengl -Wait for the message: +Wait a few seconds, then start the ros bridge (choose one option): - Waiting for the client to connect... - -Then start the ros bridge (choose one option): - - export PYTHONPATH=$PYTHONPATH:/PythonAPI/ + export PYTHONPATH=$PYTHONPATH:/PythonAPI/carla/dist/carla-.egg source ~/carla-ros-bridge/catkin_ws/devel/setup.bash # Option 1: start the ros bridge roslaunch carla_ros_bridge carla_ros_bridge.launch - # Option 2: start the ros bridge together with RVIZ - roslaunch carla_ros_bridge carla_ros_bridge_with_rviz.launch - - # Option 3: start the ros bridge together with an example ego vehicle + # Option 2: start the ros bridge together with an example ego vehicle roslaunch carla_ros_bridge carla_ros_bridge_with_example_ego_vehicle.launch ## Settings @@ -267,17 +261,19 @@ The following markers are supported in 'map'-frame: | ---------------------------------- | ------------------------------------------------------------------------------------------------------ | --------------------------- | | `/carla/debug_marker` (subscriber) | [visualization_msgs.MarkerArray](http://docs.ros.org/api/visualization_msgs/html/msg/MarkerArray.html) | draw markers in CARLA world | -## Carla Ego Vehicle - -`carla_ego_vehicle` provides a generic way to spawn an ego vehicle and attach sensors to it. You can find further documentation [here](carla_ego_vehicle/README.md). - -## Carla Infrastructure Sensors - -`carla_infrastructure` provides a generic way to spawn a set of infrastructure sensors defined in a config file. You can find further documentation [here](carla_infrastructure/README.md). -## Waypoint calculation +## Additional Functionality -To make use of the Carla waypoint calculation a ROS Node is available to get waypoints. You can find further documentation [here](carla_waypoint_publisher/README.md). +| Name | Description | +| --------------------------------- | ------------------------------------------------------------------------------------------------------- | +| [Carla Ego Vehicle](carla_ego_vehicle/README.md) | Provides a generic way to spawn an ego vehicle and attach sensors to it. | +| [Carla Infrastructure](carla_infrastructure/README.md) | Provides a generic way to spawn a set of infrastructure sensors defined in a config file. | +| [Carla Waypoint Publisher](carla_waypoint_publisher/README.md) | Provide routes and access to the Carla waypoint API | +| [Carla ROS Scenario Runner](carla_ros_scenario_runner/README.md) | ROS node that wraps the functionality of the CARLA [scenario runner](https://github.com/carla-simulator/scenario_runner) to execute scenarios. | +| [Carla AD Agent](carla_ad_agent/README.md) | A basic AD agent, that can follow a route and avoid collisions with other vehicles and stop on red traffic lights. | +| [Carla AD Demo](carla_ad_demo/README.md) | A meta package that provides everything to launch a CARLA ROS environment with an AD vehicle. | +| [RVIZ Carla Plugin](rviz_carla_plugin/README.md) | A [RVIZ](http://wiki.ros.org/rviz) plugin to visualize/control CARLA. | +| [RQT Carla Plugin](rqt_carla_plugin/README.md) | A [RQT](http://wiki.ros.org/rqt) plugin to control CARLA. | ## Troubleshooting @@ -285,7 +281,7 @@ To make use of the Carla waypoint calculation a ROS Node is available to get way You're missing Carla Python. Please execute: - export PYTHONPATH=$PYTHONPATH:/PythonAPI/ + export PYTHONPATH=$PYTHONPATH:/PythonAPI/carla/dist/ Please note that you have to put in the complete path to the egg-file including the egg-file itself. Please use the one, that is supported by your Python version. diff --git a/carla_ad_agent/CMakeLists.txt b/carla_ad_agent/CMakeLists.txt new file mode 100644 index 00000000..8f5a7216 --- /dev/null +++ b/carla_ad_agent/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 2.8.3) +project(carla_ad_agent) + +find_package(catkin REQUIRED COMPONENTS + rospy + roslaunch +) + +catkin_python_setup() + +roslaunch_add_file_check(launch) + +catkin_package( + CATKIN_DEPENDS + rospy +) + +catkin_install_python(PROGRAMS + src/carla_ad_agent/carla_ad_agent.py + src/carla_ad_agent/basic_agent.py + src/carla_ad_agent/agent.py + src/carla_ad_agent/local_planner.py + src/carla_ad_agent/vehicle_pid_controller.py + src/carla_ad_agent/misc.py + src/carla_ad_agent/__init__.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) + diff --git a/carla_ad_agent/README.md b/carla_ad_agent/README.md new file mode 100644 index 00000000..63ee8ffd --- /dev/null +++ b/carla_ad_agent/README.md @@ -0,0 +1,13 @@ +# CARLA AD Agent + +An AD agent that can follow a given route. + +It avoids crashs with other vehicles and respects the state of the traffic lights. + +For a more comprehensive solution, have a look at [Autoware](https://www.autoware.ai/). + +## Publications + +| Topic | Type | Description | +| ---------------------------------- | ------------------- | --------------------------- | +| `/carla//vehicle_control_cmd` | [carla_msgs.CarlaEgoVehicleControl](../carla_msgs/msg/CarlaEgoVehicleControl.msg) | Vehicle control command | diff --git a/carla_ad_agent/launch/carla_ad_agent.launch b/carla_ad_agent/launch/carla_ad_agent.launch new file mode 100644 index 00000000..209de6b8 --- /dev/null +++ b/carla_ad_agent/launch/carla_ad_agent.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/carla_ad_agent/package.xml b/carla_ad_agent/package.xml new file mode 100644 index 00000000..d2b3d2a5 --- /dev/null +++ b/carla_ad_agent/package.xml @@ -0,0 +1,18 @@ + + + carla_ad_agent + 0.0.1 + The carla_ad_agent package + CARLA Simulator Team + MIT + catkin + rospy + roslaunch + rospy + rospy + topic_tools + carla_waypoint_types + carla_msgs + + + diff --git a/carla_ad_agent/setup.py b/carla_ad_agent/setup.py new file mode 100644 index 00000000..0af8e3a7 --- /dev/null +++ b/carla_ad_agent/setup.py @@ -0,0 +1,10 @@ +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['carla_ad_agent'], + package_dir={'': 'src'} +) + +setup(**d) + diff --git a/carla_ad_agent/src/carla_ad_agent/__init__.py b/carla_ad_agent/src/carla_ad_agent/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/carla_ad_agent/src/carla_ad_agent/agent.py b/carla_ad_agent/src/carla_ad_agent/agent.py new file mode 100644 index 00000000..c1cc8c55 --- /dev/null +++ b/carla_ad_agent/src/carla_ad_agent/agent.py @@ -0,0 +1,290 @@ +#!/usr/bin/env python +# +# Copyright (c) 2018-2020 Intel Corporation +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . +# +""" +Base class for agent +""" + +from enum import Enum +import math +import rospy +from tf.transformations import euler_from_quaternion +from misc import is_within_distance_ahead, compute_magnitude_angle +from carla_msgs.msg import CarlaEgoVehicleControl, CarlaTrafficLightStatus,\ + CarlaTrafficLightStatusList, CarlaWorldInfo +from carla_waypoint_types.srv import GetWaypoint + + +class AgentState(Enum): + """ + AGENT_STATE represents the possible states of a roaming agent + """ + NAVIGATING = 1 + BLOCKED_BY_VEHICLE = 2 + BLOCKED_RED_LIGHT = 3 + + +class Agent(object): + """ + Base class for agent + """ + + def __init__(self, role_name, vehicle_id): + """ + """ + self._proximity_threshold = 10.0 # meters + self._local_planner = None + self._map_name = None + self._vehicle_location = None + self._vehicle_yaw = None + self._vehicle_id = vehicle_id + self._last_traffic_light = None + + rospy.wait_for_service('/carla_waypoint_publisher/{}/get_waypoint'.format(role_name)) + + self._get_waypoint_client = rospy.ServiceProxy( + '/carla_waypoint_publisher/{}/get_waypoint'.format(role_name), GetWaypoint) + + self._traffic_lights = [] + self._traffic_light_status_subscriber = rospy.Subscriber( + "/carla/traffic_lights", CarlaTrafficLightStatusList, self.traffic_lights_updated) + + self._world_info_subscriber = rospy.Subscriber( + "/carla/world_info", CarlaWorldInfo, self.world_info_updated) + + def traffic_lights_updated(self, traffic_lights): + """ + callback on new traffic light list + """ + self._traffic_lights = traffic_lights.traffic_lights + + def world_info_updated(self, world_info): + """ + callback on new world info + """ + self._map_name = world_info.map_name + + def odometry_updated(self, odo): + """ + callback on new odometry + """ + self._vehicle_location = odo.pose.pose.position + quaternion = ( + odo.pose.pose.orientation.x, + odo.pose.pose.orientation.y, + odo.pose.pose.orientation.z, + odo.pose.pose.orientation.w + ) + _, _, self._vehicle_yaw = euler_from_quaternion(quaternion) + + def run_step(self, target_speed): # pylint: disable=no-self-use,unused-argument + """ + Execute one step of navigation. + :return: control + """ + control = CarlaEgoVehicleControl() + return control + + def _is_light_red(self, lights_list): + """ + Method to check if there is a red light affecting us. This version of + the method is compatible with both European and US style traffic lights. + + :param lights_list: list containing TrafficLight objects + :return: a tuple given by (bool_flag, traffic_light), where + - bool_flag is True if there is a traffic light in RED + affecting us and False otherwise + - traffic_light is the object itself or None if there is no + red traffic light affecting us + """ + if self._map_name == 'Town01' or self._map_name == 'Town02': + return self._is_light_red_europe_style(lights_list) + else: + return self._is_light_red_us_style(lights_list) + + def _is_light_red_europe_style(self, lights_list): + """ + This method is specialized to check European style traffic lights. + + :param lights_list: list containing TrafficLight objects + :return: a tuple given by (bool_flag, traffic_light), where + - bool_flag is True if there is a traffic light in RED + affecting us and False otherwise + - traffic_light is the object itself or None if there is no + red traffic light affecting us + """ + ego_vehicle_location = self._vehicle_location + ego_vehicle_waypoint = self.get_waypoint(ego_vehicle_location) + if not ego_vehicle_waypoint: + if not rospy.is_shutdown(): + rospy.logwarn("Could not get waypoint for ego vehicle.") + return (False, None) + + for traffic_light in lights_list: + object_waypoint = traffic_light[1] + if object_waypoint.road_id != ego_vehicle_waypoint.road_id or \ + object_waypoint.lane_id != ego_vehicle_waypoint.lane_id: + continue + if is_within_distance_ahead(object_waypoint.pose.position, ego_vehicle_location, + math.degrees(self._vehicle_yaw), + self._proximity_threshold): + traffic_light_state = CarlaTrafficLightStatus.RED + for status in self._traffic_lights: + if status.id == traffic_light[0]: + traffic_light_state = status.state + break + if traffic_light_state == CarlaTrafficLightStatus.RED: + return (True, traffic_light[0]) + + return (False, None) + + def get_waypoint(self, location): + """ + Helper to get waypoint for location via ros service + """ + if rospy.is_shutdown(): + return None + try: + response = self._get_waypoint_client(location) + return response.waypoint + except (rospy.ServiceException, rospy.ROSInterruptException) as e: + if not rospy.is_shutdown(): + rospy.logwarn("Service call 'get_waypoint' failed: {}".format(e)) + + def _is_light_red_us_style(self, lights_list): # pylint: disable=too-many-branches + """ + This method is specialized to check US style traffic lights. + + :param lights_list: list containing TrafficLight objects + :return: a tuple given by (bool_flag, traffic_light), where + - bool_flag is True if there is a traffic light in RED + affecting us and False otherwise + - traffic_light is the object itself or None if there is no + red traffic light affecting us + """ + ego_vehicle_location = self._vehicle_location + ego_vehicle_waypoint = self.get_waypoint(ego_vehicle_location) + if not ego_vehicle_waypoint: + if not rospy.is_shutdown(): + rospy.logwarn("Could not get waypoint for ego vehicle.") + return (False, None) + + if ego_vehicle_waypoint.is_junction: + # It is too late. Do not block the intersection! Keep going! + return (False, None) + + if self._local_planner.target_waypoint is not None: + if self._local_planner.target_waypoint.is_junction: + min_angle = 180.0 + sel_magnitude = 0.0 # pylint: disable=unused-variable + sel_traffic_light = None + for traffic_light in lights_list: + loc = traffic_light[1] + magnitude, angle = compute_magnitude_angle(loc.pose.position, + ego_vehicle_location, + math.degrees(self._vehicle_yaw)) + if magnitude < 60.0 and angle < min(25.0, min_angle): + sel_magnitude = magnitude + sel_traffic_light = traffic_light[0] + min_angle = angle + + if sel_traffic_light is not None: + # print('=== Magnitude = {} | Angle = {} | ID = {}'.format( + # sel_magnitude, min_angle, sel_traffic_light)) + + if self._last_traffic_light is None: + self._last_traffic_light = sel_traffic_light + + state = None + for status in self._traffic_lights: + if status.id == sel_traffic_light: + state = status.state + break + if state is None: + rospy.logwarn( + "Couldn't get state of traffic light {}".format( + sel_traffic_light)) + return (False, None) + + if state == CarlaTrafficLightStatus.RED: + return (True, self._last_traffic_light) + else: + self._last_traffic_light = None + + return (False, None) + + def _is_vehicle_hazard(self, vehicle_list, objects): + """ + Check if a given vehicle is an obstacle in our way. To this end we take + into account the road and lane the target vehicle is on and run a + geometry test to check if the target vehicle is under a certain distance + in front of our ego vehicle. + + WARNING: This method is an approximation that could fail for very large + vehicles, which center is actually on a different lane but their + extension falls within the ego vehicle lane. + + :param vehicle_list: list of potential obstacle to check + :return: a tuple given by (bool_flag, vehicle), where + - bool_flag is True if there is a vehicle ahead blocking us + and False otherwise + - vehicle is the blocker object itself + """ + + ego_vehicle_location = self._vehicle_location + + ego_vehicle_waypoint = self.get_waypoint(ego_vehicle_location) + if not ego_vehicle_waypoint: + if not rospy.is_shutdown(): + rospy.logwarn("Could not get waypoint for ego vehicle.") + return (False, None) + + for target_vehicle_id in vehicle_list: + # do not account for the ego vehicle + if target_vehicle_id == self._vehicle_id: + continue + + target_vehicle_location = None + for elem in objects: + if elem.id == target_vehicle_id: + target_vehicle_location = elem.pose + break + + if not target_vehicle_location: + rospy.logwarn("Location of vehicle {} not found".format(target_vehicle_id)) + continue + + # if the object is not in our lane it's not an obstacle + target_vehicle_waypoint = self.get_waypoint(target_vehicle_location.position) + if not target_vehicle_waypoint: + if not rospy.is_shutdown(): + rospy.logwarn("Could not get waypoint for target vehicle.") + return (False, None) + if target_vehicle_waypoint.road_id != ego_vehicle_waypoint.road_id or \ + target_vehicle_waypoint.lane_id != ego_vehicle_waypoint.lane_id: + #rospy.loginfo("Vehicle {} is not in our lane".format(target_vehicle_id)) + continue + + if is_within_distance_ahead(target_vehicle_location.position, self._vehicle_location, + math.degrees(self._vehicle_yaw), + self._proximity_threshold): + return (True, target_vehicle_id) + + return (False, None) + + def emergency_stop(self): # pylint: disable=no-self-use + """ + Send an emergency stop command to the vehicle + :return: + """ + control = CarlaEgoVehicleControl() + control.steer = 0.0 + control.throttle = 0.0 + control.brake = 1.0 + control.hand_brake = False + + return control diff --git a/carla_ad_agent/src/carla_ad_agent/basic_agent.py b/carla_ad_agent/src/carla_ad_agent/basic_agent.py new file mode 100644 index 00000000..91ac8ee1 --- /dev/null +++ b/carla_ad_agent/src/carla_ad_agent/basic_agent.py @@ -0,0 +1,129 @@ +#!/usr/bin/env python +# +# Copyright (c) 2018-2020 Intel Corporation +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . +# +""" +BasicAgent implements a basic agent that navigates scenes to reach a given +target destination. This agent respects traffic lights and other vehicles. +""" + +import rospy +from nav_msgs.msg import Odometry +from derived_object_msgs.msg import ObjectArray +from carla_msgs.msg import CarlaActorList +from agent import Agent, AgentState +from local_planner import LocalPlanner +from carla_waypoint_types.srv import GetActorWaypoint + + +class BasicAgent(Agent): + """ + BasicAgent implements a basic agent that navigates scenes to reach a given + target destination. This agent respects traffic lights and other vehicles. + """ + + def __init__(self, role_name, ego_vehicle_id, avoid_risk=True): + """ + """ + super(BasicAgent, self).__init__(role_name, ego_vehicle_id) + + self._proximity_threshold = 10.0 # meters + self._state = AgentState.NAVIGATING + self._avoid_risk = avoid_risk + args_lateral_dict = { + 'K_P': 0.9, + 'K_D': 0.0, + 'K_I': 0.1} + self._local_planner = LocalPlanner(role_name, + opt_dict={'lateral_control_dict': args_lateral_dict}) + + self._vehicle_id_list = [] + self._lights_id_list = [] + self._actors_subscriber = rospy.Subscriber( + "/carla/actor_list", CarlaActorList, self.actors_updated) + self._objects = [] + self._objects_subscriber = rospy.Subscriber( + "/carla/{}/objects".format(role_name), ObjectArray, self.objects_updated) + + self._odometry_subscriber = rospy.Subscriber( + "/carla/{}/odometry".format(role_name), Odometry, self.odometry_updated) + + self._get_actor_waypoint_client = rospy.ServiceProxy( + '/carla_waypoint_publisher/{}/get_actor_waypoint'.format(role_name), GetActorWaypoint) + + def get_actor_waypoint(self, actor_id): + """ + helper method to get waypoint for actor via ros service + """ + try: + response = self._get_actor_waypoint_client(actor_id) + return response.waypoint + except (rospy.ServiceException, rospy.ROSInterruptException) as e: + if not rospy.is_shutdown: + rospy.logwarn("Service call failed: {}".format(e)) + + def odometry_updated(self, odo): + """ + callback on new odometry + """ + self._local_planner.odometry_updated(odo) + super(BasicAgent, self).odometry_updated(odo) + + def actors_updated(self, actors): + """ + callback on new actor list + """ + # retrieve relevant elements for safe navigation, i.e.: traffic lights + # and other vehicles + self._vehicle_id_list = [] + self._lights_id_list = [] + for actor in actors.actors: + if "vehicle" in actor.type: + self._vehicle_id_list.append(actor.id) + elif "traffic_light" in actor.type: + self._lights_id_list.append((actor.id, self.get_actor_waypoint(actor.id))) + + def objects_updated(self, objects): + """ + callback on new objects + """ + self._objects = objects.objects + + def run_step(self, target_speed): + """ + Execute one step of navigation. + :return: carla.VehicleControl + """ + + # is there an obstacle in front of us? + hazard_detected = False + + if self._avoid_risk: + # check possible obstacles + vehicle_state, vehicle = self._is_vehicle_hazard( # pylint: disable=unused-variable + self._vehicle_id_list, self._objects) + if vehicle_state: + #rospy.loginfo('=== Vehicle blocking ahead [{}])'.format(vehicle)) + self._state = AgentState.BLOCKED_BY_VEHICLE + hazard_detected = True + + # check for the state of the traffic lights + light_state, traffic_light = self._is_light_red( # pylint: disable=unused-variable + self._lights_id_list) + if light_state: + #rospy.loginfo('=== Red Light ahead [{}])'.format(traffic_light)) + + self._state = AgentState.BLOCKED_RED_LIGHT + hazard_detected = True + + if hazard_detected: + control = self.emergency_stop() + else: + self._state = AgentState.NAVIGATING + # standard local planner behavior + control = self._local_planner.run_step(target_speed) + + return control diff --git a/carla_ad_agent/src/carla_ad_agent/carla_ad_agent.py b/carla_ad_agent/src/carla_ad_agent/carla_ad_agent.py new file mode 100755 index 00000000..9c3abff8 --- /dev/null +++ b/carla_ad_agent/src/carla_ad_agent/carla_ad_agent.py @@ -0,0 +1,133 @@ +#!/usr/bin/env python +# +# Copyright (c) 2020 Intel Corporation +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . +# +""" +A basic AD agent using CARLA waypoints +""" +import rospy +from nav_msgs.msg import Path +from std_msgs.msg import Float64 +from carla_msgs.msg import CarlaEgoVehicleInfo, CarlaEgoVehicleControl +from basic_agent import BasicAgent + + +class CarlaAdAgent(object): + """ + A basic AD agent using CARLA waypoints + """ + + def __init__(self, role_name, target_speed, avoid_risk): + """ + Constructor + """ + self._route_assigned = False + self._global_plan = None + self._agent = None + self._target_speed = target_speed + rospy.on_shutdown(self.on_shutdown) + + # wait for ego vehicle + vehicle_info = rospy.wait_for_message( + "/carla/{}/vehicle_info".format(role_name), CarlaEgoVehicleInfo) + + self._route_subscriber = rospy.Subscriber( + "/carla/{}/waypoints".format(role_name), Path, self.path_updated) + + self._target_speed_subscriber = rospy.Subscriber( + "/carla/{}/target_speed".format(role_name), Float64, self.target_speed_updated) + + self.vehicle_control_publisher = rospy.Publisher( + "/carla/{}/vehicle_control_cmd".format(role_name), CarlaEgoVehicleControl, queue_size=1) + + self._agent = BasicAgent(role_name, vehicle_info.id, # pylint: disable=no-member + avoid_risk) + + def on_shutdown(self): + """ + callback on shutdown + """ + rospy.loginfo("Shutting down, stopping ego vehicle...") + if self._agent: + self.vehicle_control_publisher.publish(self._agent.emergency_stop()) + + def target_speed_updated(self, target_speed): + """ + callback on new target speed + """ + rospy.loginfo("New target speed received: {}".format(target_speed.data)) + self._target_speed = target_speed.data + + def path_updated(self, path): + """ + callback on new route + """ + rospy.loginfo("New plan with {} waypoints received.".format(len(path.poses))) + if self._agent: + self.vehicle_control_publisher.publish(self._agent.emergency_stop()) + self._global_plan = path + self._route_assigned = False + + def run_step(self): + """ + Execute one step of navigation. + """ + control = CarlaEgoVehicleControl() + control.steer = 0.0 + control.throttle = 0.0 + control.brake = 0.0 + control.hand_brake = False + + if not self._agent: + rospy.loginfo("Waiting for ego vehicle...") + return control + + if not self._route_assigned and self._global_plan: + rospy.loginfo("Assigning plan...") + self._agent._local_planner.set_global_plan( # pylint: disable=protected-access + self._global_plan.poses) + self._route_assigned = True + else: + control = self._agent.run_step(self._target_speed) + + return control + + def run(self): + """ + + Control loop + + :return: + """ + + while not rospy.is_shutdown(): + control = self.run_step() + if control: + control.steer = -control.steer + self.vehicle_control_publisher.publish(control) + + +def main(): + """ + + main function + + :return: + """ + rospy.init_node('carla_ad_agent', anonymous=True) + role_name = rospy.get_param("~role_name", "ego_vehicle") + target_speed = rospy.get_param("~target_speed", 20) + avoid_risk = rospy.get_param("~avoid_risk", True) + controller = CarlaAdAgent(role_name, target_speed, avoid_risk) + try: + controller.run() + finally: + del controller + rospy.loginfo("Done") + + +if __name__ == "__main__": + main() diff --git a/carla_ad_agent/src/carla_ad_agent/local_planner.py b/carla_ad_agent/src/carla_ad_agent/local_planner.py new file mode 100644 index 00000000..ab14dfca --- /dev/null +++ b/carla_ad_agent/src/carla_ad_agent/local_planner.py @@ -0,0 +1,203 @@ +#!/usr/bin/env python +# +# Copyright (c) 2018-2020 Intel Corporation +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . +# +""" +This module contains a local planner to perform +low-level waypoint following based on PID controllers. +""" + +from collections import deque +import math +import rospy +from geometry_msgs.msg import PointStamped +from geometry_msgs.msg import Pose +from tf.transformations import euler_from_quaternion +from carla_waypoint_types.srv import GetWaypoint +from carla_msgs.msg import CarlaEgoVehicleControl +from vehicle_pid_controller import VehiclePIDController +from misc import distance_vehicle + + +class LocalPlanner(object): + """ + LocalPlanner implements the basic behavior of following a trajectory of waypoints that is + generated on-the-fly. The low-level motion of the vehicle is computed by using two PID + controllers, one is used for the lateral control and the other for the longitudinal + control (cruise speed). + + When multiple paths are available (intersections) this local planner makes a random choice. + """ + + # minimum distance to target waypoint as a percentage (e.g. within 90% of + # total distance) + MIN_DISTANCE_PERCENTAGE = 0.9 + + def __init__(self, role_name, opt_dict=None): + """ + :param vehicle: actor to apply to local planner logic onto + :param opt_dict: dictionary of arguments with the following semantics: + + target_speed -- desired cruise speed in Km/h + + sampling_radius -- search radius for next waypoints in seconds: e.g. 0.5 seconds ahead + + lateral_control_dict -- dictionary of arguments to setup the lateral PID controller + {'K_P':, 'K_D':, 'K_I'} + + longitudinal_control_dict -- dictionary of arguments to setup the longitudinal + PID controller + {'K_P':, 'K_D':, 'K_I'} + """ + self._current_waypoint = None + self.target_waypoint = None + self._vehicle_controller = None + self._global_plan = None + self._waypoints_queue = deque(maxlen=20000) + self._buffer_size = 5 + self._waypoint_buffer = deque(maxlen=self._buffer_size) + self._vehicle_yaw = None + self._current_speed = None + self._current_pose = None + + self._target_point_publisher = rospy.Publisher( + "/next_target", PointStamped, queue_size=1) + rospy.wait_for_service('/carla_waypoint_publisher/{}/get_waypoint'.format(role_name)) + + self._get_waypoint_client = rospy.ServiceProxy( + '/carla_waypoint_publisher/{}/get_waypoint'.format(role_name), GetWaypoint) + + # initializing controller + self._init_controller(opt_dict) + + def get_waypoint(self, location): + """ + Helper to get waypoint from a ros service + """ + try: + response = self._get_waypoint_client(location) + return response.waypoint + except (rospy.ServiceException, rospy.ROSInterruptException) as e: + if not rospy.is_shutdown: + rospy.logwarn("Service call failed: {}".format(e)) + + def odometry_updated(self, odo): + """ + Callback on new odometry + """ + self._current_speed = math.sqrt(odo.twist.twist.linear.x ** 2 + + odo.twist.twist.linear.y ** 2 + + odo.twist.twist.linear.z ** 2) * 3.6 + + self._current_pose = odo.pose.pose + quaternion = ( + odo.pose.pose.orientation.x, + odo.pose.pose.orientation.y, + odo.pose.pose.orientation.z, + odo.pose.pose.orientation.w + ) + _, _, self._vehicle_yaw = euler_from_quaternion(quaternion) + + def _init_controller(self, opt_dict): + """ + Controller initialization. + + :param opt_dict: dictionary of arguments. + :return: + """ + # default params + self._current_speed = 0.0 # Km/h + self._current_pose = Pose() + args_lateral_dict = { + 'K_P': 1.95, + 'K_D': 0.01, + 'K_I': 1.4} + args_longitudinal_dict = { + 'K_P': 0.2, + 'K_D': 0.05, + 'K_I': 0.1} + + # parameters overload + if opt_dict: + if 'lateral_control_dict' in opt_dict: + args_lateral_dict = opt_dict['lateral_control_dict'] + if 'longitudinal_control_dict' in opt_dict: + args_longitudinal_dict = opt_dict['longitudinal_control_dict'] + + self._current_waypoint = self.get_waypoint(self._current_pose.position) + self._vehicle_controller = VehiclePIDController(args_lateral=args_lateral_dict, + args_longitudinal=args_longitudinal_dict) + + self._global_plan = False + + def set_global_plan(self, current_plan): + """ + set a global plan to follow + """ + self._waypoints_queue.clear() + self._waypoint_buffer.clear() + for elem in current_plan: + self._waypoints_queue.append(elem.pose) + self._global_plan = True + + def run_step(self, target_speed): + """ + Execute one step of local planning which involves running the longitudinal + and lateral PID controllers to follow the waypoints trajectory. + """ + if not self._waypoints_queue: + control = CarlaEgoVehicleControl() + control.steer = 0.0 + control.throttle = 0.0 + control.brake = 1.0 + control.hand_brake = False + control.manual_gear_shift = False + + return control + + # Buffering the waypoints + if not self._waypoint_buffer: + for i in range(self._buffer_size): + if self._waypoints_queue: + self._waypoint_buffer.append( + self._waypoints_queue.popleft()) + else: + break + + # current vehicle waypoint + self._current_waypoint = self.get_waypoint(self._current_pose.position) + + # target waypoint + target_route_point = self._waypoint_buffer[0] + + # for us redlight-detection + self.target_waypoint = self.get_waypoint(target_route_point.position) + + target_point = PointStamped() + target_point.header.frame_id = "map" + target_point.point.x = target_route_point.position.x + target_point.point.y = target_route_point.position.y + target_point.point.z = target_route_point.position.z + self._target_point_publisher.publish(target_point) + # move using PID controllers + control = self._vehicle_controller.run_step( + target_speed, self._current_speed, self._current_pose, target_route_point) + + # purge the queue of obsolete waypoints + max_index = -1 + + sampling_radius = target_speed * 1 / 3.6 # 1 seconds horizon + min_distance = sampling_radius * self.MIN_DISTANCE_PERCENTAGE + + for i, route_point in enumerate(self._waypoint_buffer): + if distance_vehicle( + route_point, self._current_pose.position) < min_distance: + max_index = i + if max_index >= 0: + for i in range(max_index + 1): + self._waypoint_buffer.popleft() + + return control diff --git a/carla_ad_agent/src/carla_ad_agent/misc.py b/carla_ad_agent/src/carla_ad_agent/misc.py new file mode 100644 index 00000000..9495b815 --- /dev/null +++ b/carla_ad_agent/src/carla_ad_agent/misc.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python +# +# Copyright (c) 2018-2020 Intel Corporation +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . +# +""" This module contains helper functions """ + +import math +import numpy as np + + +def is_within_distance_ahead(target_location, current_location, orientation, max_distance): + """ + Check if a target object is within a certain distance in front of a reference object. + + :param target_location: location of the target object + :param current_location: location of the reference object + :param orientation: orientation of the reference object + :param max_distance: maximum allowed distance + :return: True if target object is within max_distance ahead of the reference object + """ + target_vector = np.array([target_location.x - current_location.x, + target_location.y - current_location.y]) + norm_target = np.linalg.norm(target_vector) + + # If the vector is too short, we can simply stop here + if norm_target < 0.001: + return True + + if norm_target > max_distance: + return False + + forward_vector = np.array( + [math.cos(math.radians(orientation)), math.sin(math.radians(orientation))]) + d_angle = math.degrees(math.acos(np.dot(forward_vector, target_vector) / norm_target)) + + return d_angle < 90.0 + + +def compute_magnitude_angle(target_location, current_location, orientation): + """ + Compute relative angle and distance between a target_location and a current_location + + :param target_location: location of the target object + :param current_location: location of the reference object + :param orientation: orientation of the reference object + :return: a tuple composed by the distance to the object and the angle between both objects + """ + target_vector = np.array([target_location.x - current_location.x, + target_location.y - current_location.y]) + norm_target = np.linalg.norm(target_vector) + + forward_vector = np.array([math.cos(math.radians(orientation)), + math.sin(math.radians(orientation))]) + d_angle = math.degrees( + math.acos(np.clip(np.dot(forward_vector, target_vector) / norm_target, -1., 1.))) + + return (norm_target, d_angle) + + +def distance_vehicle(waypoint, vehicle_position): + """ + calculate distance between waypoint and vehicle position + """ + dx = waypoint.position.x - vehicle_position.x + dy = waypoint.position.y - vehicle_position.y + + return math.sqrt(dx * dx + dy * dy) diff --git a/carla_ad_agent/src/carla_ad_agent/vehicle_pid_controller.py b/carla_ad_agent/src/carla_ad_agent/vehicle_pid_controller.py new file mode 100644 index 00000000..6d19936d --- /dev/null +++ b/carla_ad_agent/src/carla_ad_agent/vehicle_pid_controller.py @@ -0,0 +1,170 @@ +#!/usr/bin/env python +# +# Copyright (c) 2018-2020 Intel Corporation +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . +# + +""" This module contains PID controllers to perform lateral and longitudinal control. """ + +from collections import deque +import math +import numpy as np +import rospy +from tf.transformations import euler_from_quaternion +from geometry_msgs.msg import Point +from carla_msgs.msg import CarlaEgoVehicleControl + + +class VehiclePIDController(object): # pylint: disable=too-few-public-methods + """ + VehiclePIDController is the combination of two PID controllers (lateral and longitudinal) + to perform the low level control a vehicle from client side + """ + + def __init__(self, args_lateral=None, args_longitudinal=None): + """ + :param vehicle: actor to apply to local planner logic onto + :param args_lateral: dictionary of arguments to set the lateral PID controller using + the following semantics: + K_P -- Proportional term + K_D -- Differential term + K_I -- Integral term + :param args_longitudinal: dictionary of arguments to set the longitudinal PID + controller using the following semantics: + K_P -- Proportional term + K_D -- Differential term + K_I -- Integral term + """ + if not args_lateral: + args_lateral = {'K_P': 1.0, 'K_D': 0.0, 'K_I': 0.0} + if not args_longitudinal: + args_longitudinal = {'K_P': 1.0, 'K_D': 0.0, 'K_I': 0.0} + + self._lon_controller = PIDLongitudinalController(**args_longitudinal) + self._lat_controller = PIDLateralController(**args_lateral) + self._last_control_time = rospy.get_time() + + def run_step(self, target_speed, current_speed, current_pose, waypoint): + """ + Execute one step of control invoking both lateral and longitudinal + PID controllers to reach a target waypoint at a given target_speed. + + :param target_speed: desired vehicle speed + :param waypoint: target location encoded as a waypoint + :return: distance (in meters) to the waypoint + """ + current_time = rospy.get_time() + dt = current_time-self._last_control_time + if dt == 0.0: + dt = 0.000001 + control = CarlaEgoVehicleControl() + throttle = self._lon_controller.run_step(target_speed, current_speed, dt) + steering = self._lat_controller.run_step(current_pose, waypoint, dt) + self._last_control_time = current_time + control.steer = steering + control.throttle = throttle + control.brake = 0.0 + control.hand_brake = False + control.manual_gear_shift = False + + return control + + +class PIDLongitudinalController(object): # pylint: disable=too-few-public-methods + """ + PIDLongitudinalController implements longitudinal control using a PID. + """ + + def __init__(self, K_P=1.0, K_D=0.0, K_I=0.0): + """ + :param vehicle: actor to apply to local planner logic onto + :param K_P: Proportional term + :param K_D: Differential term + :param K_I: Integral term + """ + self._K_P = K_P + self._K_D = K_D + self._K_I = K_I + self._e_buffer = deque(maxlen=30) + + def run_step(self, target_speed, current_speed, dt): + """ + Estimate the throttle of the vehicle based on the PID equations + + :param target_speed: target speed in Km/h + :param current_speed: current speed of the vehicle in Km/h + :return: throttle control in the range [0, 1] + """ + _e = (target_speed - current_speed) + self._e_buffer.append(_e) + + if len(self._e_buffer) >= 2: + _de = (self._e_buffer[-1] - self._e_buffer[-2]) / dt + _ie = sum(self._e_buffer) * dt + else: + _de = 0.0 + _ie = 0.0 + + return np.clip((self._K_P * _e) + (self._K_D * _de / dt) + (self._K_I * _ie * dt), 0.0, 1.0) + + +class PIDLateralController(object): # pylint: disable=too-few-public-methods + """ + PIDLateralController implements lateral control using a PID. + """ + + def __init__(self, K_P=1.0, K_D=0.0, K_I=0.0): + """ + :param vehicle: actor to apply to local planner logic onto + :param K_P: Proportional term + :param K_D: Differential term + :param K_I: Integral term + """ + self._K_P = K_P + self._K_D = K_D + self._K_I = K_I + self._e_buffer = deque(maxlen=10) + + def run_step(self, current_pose, waypoint, dt): + """ + Estimate the steering angle of the vehicle based on the PID equations + + :param waypoint: target waypoint + :param current_pose: current pose of the vehicle + :return: steering control in the range [-1, 1] + """ + v_begin = current_pose.position + quaternion = ( + current_pose.orientation.x, + current_pose.orientation.y, + current_pose.orientation.z, + current_pose.orientation.w + ) + _, _, yaw = euler_from_quaternion(quaternion) + v_end = Point() + v_end.x = v_begin.x + math.cos(yaw) + v_end.y = v_begin.y + math.sin(yaw) + + v_vec = np.array([v_end.x - v_begin.x, v_end.y - v_begin.y, 0.0]) + w_vec = np.array([waypoint.position.x - + v_begin.x, waypoint.position.y - + v_begin.y, 0.0]) + _dot = math.acos(np.clip(np.dot(w_vec, v_vec) / + (np.linalg.norm(w_vec) * np.linalg.norm(v_vec)), -1.0, 1.0)) + + _cross = np.cross(v_vec, w_vec) + if _cross[2] < 0: + _dot *= -1.0 + + self._e_buffer.append(_dot) + if len(self._e_buffer) >= 2: + _de = (self._e_buffer[-1] - self._e_buffer[-2]) / dt + _ie = sum(self._e_buffer) * dt + else: + _de = 0.0 + _ie = 0.0 + + return np.clip((self._K_P * _dot) + (self._K_D * _de / + dt) + (self._K_I * _ie * dt), -1.0, 1.0) diff --git a/carla_ad_demo/CMakeLists.txt b/carla_ad_demo/CMakeLists.txt new file mode 100644 index 00000000..56b4a771 --- /dev/null +++ b/carla_ad_demo/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 2.8.3) +project(carla_ad_demo) + +find_package(catkin REQUIRED COMPONENTS + roslaunch +) + + +roslaunch_add_file_check(launch) + +catkin_package() + +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) + +install(DIRECTORY config/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config +) diff --git a/carla_ad_demo/README.md b/carla_ad_demo/README.md new file mode 100644 index 00000000..e6b17f51 --- /dev/null +++ b/carla_ad_demo/README.md @@ -0,0 +1,36 @@ +# CARLA AD Demo + +This meta package provides everything to launch a CARLA ROS environment with an AD vehicle. + + +![CARLA AD Demo](../docs/images/rviz_carla_plugin.png "rviz carla plugin") + + +## Startup + + export PYTHONPATH=$PYTHONPATH:/PythonAPI/carla-.egg:/PythonAPI/carla/ + export SCENARIO_RUNNER_PATH:= + roslaunch carla_ad_demo carla_ad_demo_with_rviz.launch + +### Modes + +#### Following A Random Route + +On startup, an ego vehicle is spawned and follows a random route to a goal. + +You might want to spawn additional vehicles (or pedestrians) by manually executing: + + /PythonAPI/examples/spawn_npc.py + +You can modify start position and goal within the [launch file](launch/carla_ad_demo.launch). The route is currently randomly (regarding the left/right turns) calculated. + +#### Scenario Execution + +If you prefer to execute a predefined scenario, select to example scenario "FollowLeadingVehicle" within the RVIZ Carla Plugin and press "Execute". The ego vehicle gets repositioned and the scenario is processed. + +You can specify your own scenarios by publishing to `/carla/available_scenarios`. See the [launch file](launch/carla_ad_demo_with_rviz.launch) for an example. + + +## Troubleshooting + +If the example scenario fails, please analyze the ros log and follow the scenario runner [Getting Started](https://github.com/carla-simulator/scenario_runner/blob/master/Docs/getting_started.md) to verify that it's working standalone. diff --git a/carla_ad_demo/config/FollowLeadingVehicle.xosc b/carla_ad_demo/config/FollowLeadingVehicle.xosc new file mode 100644 index 00000000..db33827d --- /dev/null +++ b/carla_ad_demo/config/FollowLeadingVehicle.xosc @@ -0,0 +1,270 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + +
+ + + + + +
+ + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/carla_ad_demo/config/carla_ad_demo.rviz b/carla_ad_demo/config/carla_ad_demo.rviz new file mode 100644 index 00000000..35fd2f9d --- /dev/null +++ b/carla_ad_demo/config/carla_ad_demo.rviz @@ -0,0 +1,173 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 78 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Camera + - Class: rviz_carla_plugin/CarlaControl + Name: CarlaControl +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /carla/ego_vehicle/waypoints + Unreliable: false + Value: true + - Class: rviz/Camera + Enabled: true + Image Rendering: background and overlay + Image Topic: /carla/ego_vehicle/camera/rgb/spectator_view/image_color + Name: Camera + Overlay Alpha: 0.5 + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + Visibility: + Grid: true + Marker: true + Path: true + Value: true + Zoom Factor: 1 + - Class: rviz/Marker + Enabled: true + Marker Topic: /carla/marker + Name: Marker + Namespaces: + "": true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/ThirdPersonFollower + Distance: 13.610654830932617 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.49539870023727417 + Target Frame: ego_vehicle + Value: ThirdPersonFollower (rviz) + Yaw: 3.1504008769989014 + Saved: ~ +Window Geometry: + Camera: + collapsed: false + CarlaControl: + collapsed: false + Displays: + collapsed: false + Height: 1050 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001fe00000367fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006800fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000046000000e6000000e600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002870000001a0000000000000000fb0000000a0049006d00610067006501000002a70000001a0000000000000000fb0000000a0049006d00610067006501000002c70000001a0000000000000000fb0000000c00430061006d0065007200610100000132000001960000001a00fffffffb00000018004300610072006c00610043006f006e00740072006f006c01000002ce000000df000000df00ffffff000000010000010f00000367fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004600000367000000be00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e00000044fc0100000002fb0000000800540069006d006501000000000000077e000002e400fffffffb0000000800540069006d00650100000000000004500000000000000000000004650000036700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1918 + X: 0 + Y: 27 diff --git a/carla_ad_demo/config/sensors.json b/carla_ad_demo/config/sensors.json new file mode 100644 index 00000000..3d8fe104 --- /dev/null +++ b/carla_ad_demo/config/sensors.json @@ -0,0 +1,48 @@ +{ + "sensors": [ + { + "type": "sensor.camera.rgb", + "id": "view", + "x": -4.5, "y": 0.0, "z": 2.8, "roll": 0.0, "pitch": -20.0, "yaw": 0.0, + "width": 800, + "height": 600, + "fov": 100, + "sensor_tick": 0.05, + "gamma": 2.2, + "shutter_speed": 60.0, + "iso": 1200.0, + "fstop": 1.4, + "min_fstop": 1.2, + "blade_count": 5, + "exposure_mode": "manual", + "exposure_compensation": 3.0, + "exposure_min_bright": 0.1, + "exposure_max_bright": 2.0, + "exposure_speed_up": 3.0, + "exposure_speed_down": 1.0, + "calibration_constant": 16.0, + "focal_distance": 1000.0, + "blur_amount": 1.0, + "blur_radius": 0.0, + "motion_blur_intensity": 0.45, + "motion_blur_max_distortion": 0.35, + "motion_blur_min_object_screen_size": 0.1, + "slope": 0.88, + "toe": 0.55, + "shoulder": 0.26, + "black_clip": 0.0, + "white_clip": 0.04, + "temp": 6500.0, + "tint": 0.0, + "chromatic_aberration_intensity": 0.0, + "chromatic_aberration_offset": 0.0, + "enable_postprocess_effects": "True", + "lens_circle_falloff": 5.0, + "lens_circle_multiplier": 0.0, + "lens_k": -1.0, + "lens_kcube": 0.0, + "lens_x_size": 0.08, + "lens_y_size": 0.08 + } + ] +} diff --git a/carla_ad_demo/launch/carla_ad_demo.launch b/carla_ad_demo/launch/carla_ad_demo.launch new file mode 100644 index 00000000..c679c7bf --- /dev/null +++ b/carla_ad_demo/launch/carla_ad_demo.launch @@ -0,0 +1,71 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/carla_ad_demo/launch/carla_ad_demo_with_rviz.launch b/carla_ad_demo/launch/carla_ad_demo_with_rviz.launch new file mode 100644 index 00000000..2efff80c --- /dev/null +++ b/carla_ad_demo/launch/carla_ad_demo_with_rviz.launch @@ -0,0 +1,84 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/carla_ad_demo/package.xml b/carla_ad_demo/package.xml new file mode 100644 index 00000000..4d354b9e --- /dev/null +++ b/carla_ad_demo/package.xml @@ -0,0 +1,20 @@ + + + carla_ad_demo + 0.0.1 + The carla_ad_demo package + CARLA Simulator Team + MIT + catkin + carla_ros_bridge + carla_ego_vehicle + carla_waypoint_publisher + carla_ad_agent + carla_manual_control + rviz_carla_plugin + carla_twist_to_control + carla_spectator_camera + carla_ros_scenario_runner + + + diff --git a/carla_ros_scenario_runner/CMakeLists.txt b/carla_ros_scenario_runner/CMakeLists.txt new file mode 100644 index 00000000..877653c6 --- /dev/null +++ b/carla_ros_scenario_runner/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 2.8.3) +project(carla_ros_scenario_runner) + +find_package(catkin REQUIRED COMPONENTS + roslaunch +) + +catkin_python_setup() + +roslaunch_add_file_check(launch) + +catkin_package() + +catkin_install_python(PROGRAMS + src/carla_ros_scenario_runner/carla_ros_scenario_runner.py + src/carla_ros_scenario_runner/application_runner.py + src/carla_ros_scenario_runner/scenario_runner_runner.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) diff --git a/carla_ros_scenario_runner/README.md b/carla_ros_scenario_runner/README.md new file mode 100644 index 00000000..ab803b35 --- /dev/null +++ b/carla_ros_scenario_runner/README.md @@ -0,0 +1,45 @@ +# ROS Scenario Runner + +This is a wrapper to execute OpenScenarios with the CARLA [scenario runner](https://github.com/carla-simulator/scenario_runner) via ROS. + +It is best used from within the [rviz_carla_plugin](../rviz_carla_plugin). + +## Setup + +Please follow the scenario runner [Getting Started](https://github.com/carla-simulator/scenario_runner/blob/master/Docs/getting_started.md) and verify, that scenario_runner is working, if started manually. + +Additionally, please execute: + + sudo apt install python-pexpect + +## Startup + +The environment variables are forwarded to scenario_runner, therefore set them to: + + export PYTHONPATH=$PYTHONPATH:/PythonAPI/carla-.egg:/PythonAPI/carla/ + +To run the ROS node: + + roslaunch carla_ros_scenario_runner carla_ros_scenario_runner.launch scenario_runner_path:= + +To run a scenario from commandline: + + rosservice call /scenario_runner/execute_scenario "{ 'scenario': { 'scenario_file': '', 'target_speed': 10.0, 'destination': { 'position': { 'x': 10, 'y': 10, 'z':0 } } } }" + + +## Available services + +| Service | Description | Type | +| ----------------------------------------------------------- | ----------- | -------------------------------------------------------------------- | +| `/scenario_runner/execute_scenario` | Execute a scenario. If another scenario is currently running, it gets stopped. | [carla_ros_scenario_runner_types.ExecuteScenario](../carla_ros_scenario_runner_types/srv/ExecuteScenario.srv) | + + +## Available topics + + +| Topic | Description | Type | +| ------------------------------------- | ----------- | -------------------------------------------------------------------- | +| `/carla//target_speed` | The ego vehicle target speed | [std_msgs.Float64](http://docs.ros.org/api/std_msgs/html/msg/Float64.html) | +| `/carla//goal` | The ego vehicle destination (e.g. used by [carla_waypoint_publisher](../carla_waypoint_publisher)) | [geometry_msgs.PoseStamped](http://docs.ros.org/api/geometry_msgs/html/msg/PoseStamped.html) | +| `/scenario_runner/status` | The current status of the scenario runner execution (e.g. used by the [rviz_carla_plugin](../rviz_carla_plugin)) | [carla_ros_scenario_runner_types.CarlaScenarioRunnerStatus](../carla_ros_scenario_runner_types/msg/CarlaScenarioRunnerStatus.msg) | + diff --git a/carla_ros_scenario_runner/launch/carla_ros_scenario_runner.launch b/carla_ros_scenario_runner/launch/carla_ros_scenario_runner.launch new file mode 100644 index 00000000..def00183 --- /dev/null +++ b/carla_ros_scenario_runner/launch/carla_ros_scenario_runner.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/carla_ros_scenario_runner/package.xml b/carla_ros_scenario_runner/package.xml new file mode 100644 index 00000000..a7701fe1 --- /dev/null +++ b/carla_ros_scenario_runner/package.xml @@ -0,0 +1,19 @@ + + + carla_ros_scenario_runner + 0.0.0 + The carla_ros_scenario_runner package + CARLA Simulator Team + MIT + catkin + rospy + roslaunch + rospy + rospy + carla_msgs + carla_ros_scenario_runner_types + std_msgs + + + + diff --git a/carla_ros_scenario_runner/setup.py b/carla_ros_scenario_runner/setup.py new file mode 100644 index 00000000..1204b037 --- /dev/null +++ b/carla_ros_scenario_runner/setup.py @@ -0,0 +1,13 @@ +""" +Setup for carla_ros_scenario_runner +""" + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['carla_ros_scenario_runner'], + package_dir={'': 'src'} +) + +setup(**d) diff --git a/carla_ros_scenario_runner/src/carla_ros_scenario_runner/__init__.py b/carla_ros_scenario_runner/src/carla_ros_scenario_runner/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/carla_ros_scenario_runner/src/carla_ros_scenario_runner/application_runner.py b/carla_ros_scenario_runner/src/carla_ros_scenario_runner/application_runner.py new file mode 100755 index 00000000..dcff47b1 --- /dev/null +++ b/carla_ros_scenario_runner/src/carla_ros_scenario_runner/application_runner.py @@ -0,0 +1,152 @@ +#!/usr/bin/env python +# +# Copyright (c) 2020 Intel Corporation +# +""" +Execute an application. +""" +import os +from enum import Enum +from threading import Thread, Event +from datetime import datetime, timedelta +import pexpect + + +class ApplicationStatus(Enum): + """ + States of an application + """ + STOPPED = 0 + STARTING = 1 + RUNNING = 2 + SHUTTINGDOWN = 3 + ERROR = 4 + + +class ApplicationRunner(object): + + """ + Execute application + """ + + def __init__(self, status_updated_fct, log_fct, ready_string=""): + """ + Constructor + """ + self._app_thread = None + self._status_updated_fct = status_updated_fct + self._log_fct = log_fct + self._shutdown_requested_event = Event() + self._ready_string = ready_string + + def execute(self, cmdline, env=None, cwd=None): + """ + Starts a thread to execute the application + """ + if self.is_running(): + self._log_fct("Application already running!") + return False + + self._shutdown_requested_event.clear() + self._app_thread = Thread(target=self.start_and_run, args=(cmdline, + env, + cwd, + self._shutdown_requested_event, + self._ready_string, + self._status_updated_fct, + self._log_fct,)) + self._app_thread.start() + + return True + + def start_and_run(self, cmdline, env, cwd, shutdown_requested_event, ready_string, # pylint: disable=too-many-arguments + status_updated_fct, log_fct): + """ + thread function + """ + status_updated_fct(ApplicationStatus.STARTING) + try: + process = self.start_process(cmdline, log_fct, env=env, cwd=cwd) + self.run(process, shutdown_requested_event, ready_string, status_updated_fct, log_fct) + except (KeyError, pexpect.ExceptionPexpect) as e: + self._log_fct("Error while starting process: {}".format(e)) + status_updated_fct(ApplicationStatus.ERROR) + + def is_running(self): + """ + returns if the application is still running + """ + if self._app_thread is None: + return False + + return self._app_thread.is_alive() + + def shutdown(self): + """ + Shut down the application thread + """ + if not self.is_running(): + return + self._log_fct("Requesting shutdown...") + self._status_updated_fct(ApplicationStatus.SHUTTINGDOWN) + self._shutdown_requested_event.set() + if self._app_thread: + self._app_thread.join() + self._log_fct("Shutdown finished.") + + def start_process(self, argument_list, log_fct, env=None, cwd=None): # pylint: disable=no-self-use + """ + Starts a process. + """ + if not argument_list: + raise KeyError("No arguments given!") + executable = argument_list[0] + if not os.path.isfile(executable): + raise KeyError("The executable {} does not exist".format(executable)) + log_fct("Executing: {}".format(" ".join(argument_list))) + process = pexpect.spawn(" ".join(argument_list), env=env, cwd=cwd) + #process.logfile_read = sys.stdout + return process + + def run(self, process, shutdown_requested_event, ready_string, status_updated_fct, log_fct): # pylint: disable=no-self-use,too-many-arguments + """ + Threaded application execution + + :return: + """ + shutting_down_trigger_time = None + signaled_running = False + while True: + if shutdown_requested_event.is_set(): + if shutting_down_trigger_time is None: + shutting_down_trigger_time = datetime.now() + log_fct("Shutdown requested while process is still \ + running. Sending SIGHUP/SIGINT...") + process.terminate(force=False) + else: + if (datetime.now() - shutting_down_trigger_time) > timedelta(seconds=8): + log_fct("Waited 8s for application to exit. Forcing Shutdown. \ + Sending SIGKILL") + process.terminate(force=True) + try: + process.expect(".*\n", timeout=0.1) + log_fct(process.after.strip()) + if not signaled_running: + if process.after.find(ready_string) != -1: + status_updated_fct(ApplicationStatus.RUNNING) + log_fct("Application is ready.") + signaled_running = True + except pexpect.EOF: + # application exited + log_fct(process.before.strip()) + log_fct("Application exited. Exiting run loop") + break + except pexpect.TIMEOUT: + # no output received + pass + + process.close() + if process.exitstatus == 0: + status_updated_fct(ApplicationStatus.STOPPED) + else: + status_updated_fct(ApplicationStatus.ERROR) diff --git a/carla_ros_scenario_runner/src/carla_ros_scenario_runner/carla_ros_scenario_runner.py b/carla_ros_scenario_runner/src/carla_ros_scenario_runner/carla_ros_scenario_runner.py new file mode 100755 index 00000000..77ae976e --- /dev/null +++ b/carla_ros_scenario_runner/src/carla_ros_scenario_runner/carla_ros_scenario_runner.py @@ -0,0 +1,169 @@ +#!/usr/bin/env python +# +# Copyright (c) 2020 Intel Corporation. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . +# +""" +Execute scenarios via ros service + +Internally, the CARLA scenario runner is executed +""" +import sys +import os +try: + import queue +except ImportError: + import Queue as queue +import rospy +from geometry_msgs.msg import PoseStamped +from std_msgs.msg import Float64 +from carla_ros_scenario_runner_types.srv import ExecuteScenario, ExecuteScenarioResponse +from carla_ros_scenario_runner_types.msg import CarlaScenarioRunnerStatus +from application_runner import ApplicationStatus +from scenario_runner_runner import ScenarioRunnerRunner + +# Check Python dependencies of scenario runner +try: + import carla # pylint: disable=unused-import +except ImportError: + print "ERROR: CARLA Python Egg not available. Please add \ + /PythonAPI/carla/dist/carla--\ + py-linux-x86_64.egg to your PYTHONPATH." + sys.exit(1) + +try: + from agents.navigation.local_planner import LocalPlanner # pylint: disable=unused-import +except ImportError: + print "ERROR: CARLA Python Agents not available. \ + Please add /PythonAPI/carla to your PYTHONPATH." + sys.exit(1) + + +class CarlaRosScenarioRunner(object): + """ + Execute scenarios via ros service + """ + + def __init__(self, role_name, host, scenario_runner_path): + """ + Constructor + """ + self._goal_publisher = rospy.Publisher( + "/carla/{}/goal".format(role_name), PoseStamped, queue_size=1, latch=True) + self._target_speed_publisher = rospy.Publisher( + "/carla/{}/target_speed".format(role_name), Float64, queue_size=1, latch=True) + self._status_publisher = rospy.Publisher( + "/scenario_runner/status", CarlaScenarioRunnerStatus, queue_size=1, latch=True) + self.scenario_runner_status_updated(ApplicationStatus.STOPPED) + self._scenario_runner = ScenarioRunnerRunner( + scenario_runner_path, + host, + self.scenario_runner_status_updated, + self.scenario_runner_log) + self._request_queue = queue.Queue() + self._execute_scenario_service = rospy.Service( + '/scenario_runner/execute_scenario', ExecuteScenario, self.execute_scenario) + + def scenario_runner_log(self, log): # pylint: disable=no-self-use + """ + Callback for application logs + """ + rospy.logwarn("[SC]{}".format(log)) + + def scenario_runner_status_updated(self, status): + """ + Executed from application runner whenever the status changed + """ + rospy.loginfo("Status updated to {}".format(status)) + val = CarlaScenarioRunnerStatus.STOPPED + if status == ApplicationStatus.STOPPED: + val = CarlaScenarioRunnerStatus.STOPPED + elif status == ApplicationStatus.STARTING: + val = CarlaScenarioRunnerStatus.STARTING + elif status == ApplicationStatus.RUNNING: + val = CarlaScenarioRunnerStatus.RUNNING + elif status == ApplicationStatus.SHUTTINGDOWN: + val = CarlaScenarioRunnerStatus.SHUTTINGDOWN + else: + val = CarlaScenarioRunnerStatus.ERROR + status = CarlaScenarioRunnerStatus() + status.status = val + self._status_publisher.publish(status) + + def execute_scenario(self, req): + """ + Execute a scenario + """ + rospy.loginfo("Scenario Execution requested...") + + response = ExecuteScenarioResponse() + response.result = True + if not os.path.isfile(req.scenario.scenario_file): + rospy.logwarn("Requested scenario file not existing {}".format( + req.scenario.scenario_file)) + response.result = False + else: + self._request_queue.put(req.scenario) + return response + + def run(self): + """ + Control loop + + :return: + """ + current_req = None + while not rospy.is_shutdown(): + if current_req: + if self._scenario_runner.is_running(): + rospy.loginfo("Scenario Runner currently running. Shutting down.") + self._scenario_runner.shutdown() + rospy.loginfo("Scenario Runner stopped.") + rospy.loginfo("Executing scenario {}...".format(current_req.name)) + # publish goal + goal = PoseStamped(pose=current_req.destination) + goal.header.stamp = rospy.get_rostime() + goal.header.frame_id = "map" + self._goal_publisher.publish(goal) + # publish target speed + self._target_speed_publisher.publish(Float64(data=current_req.target_speed)) + # execute scenario + scenario_executed = self._scenario_runner.execute_scenario( + current_req.scenario_file) + if not scenario_executed: + rospy.logwarn("Unable to execute scenario.") + current_req = None + else: + try: + current_req = self._request_queue.get(block=True, timeout=0.5) + except queue.Empty: + # no new request + pass + if self._scenario_runner.is_running(): + rospy.loginfo("Scenario Runner currently running. Shutting down.") + self._scenario_runner.shutdown() + + +def main(): + """ + + main function + + :return: + """ + rospy.init_node('carla_ros_scenario_runner', anonymous=True) + role_name = rospy.get_param("~role_name", "ego_vehicle") + scenario_runner_path = rospy.get_param("~scenario_runner_path", "") + host = rospy.get_param("~host", "localhost") + scenario_runner = CarlaRosScenarioRunner(role_name, host, scenario_runner_path) + try: + scenario_runner.run() + finally: + del scenario_runner + rospy.loginfo("Done.") + + +if __name__ == "__main__": + main() diff --git a/carla_ros_scenario_runner/src/carla_ros_scenario_runner/scenario_runner_runner.py b/carla_ros_scenario_runner/src/carla_ros_scenario_runner/scenario_runner_runner.py new file mode 100755 index 00000000..9ff8da61 --- /dev/null +++ b/carla_ros_scenario_runner/src/carla_ros_scenario_runner/scenario_runner_runner.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python +# +# Copyright (c) 2020 Intel Corporation +# +""" +Executes scenario runner +""" +import os +from application_runner import ApplicationRunner + + +class ScenarioRunnerRunner(ApplicationRunner): + """ + Executes scenario runner + """ + + def __init__(self, path, host, status_updated_fct, log_fct): + self._path = path + self._host = host + super(ScenarioRunnerRunner, self).__init__( + status_updated_fct, + log_fct, + "ScenarioManager: Running scenario OpenScenario") + + def execute_scenario(self, scenario_file): + """ + Executes scenario + """ + cmdline = ["/usr/bin/python", "{}/scenario_runner.py".format(self._path), + "--openscenario", "{}".format(scenario_file), + "--waitForEgo", + "--host", self._host] + return self.execute(cmdline, env=os.environ) diff --git a/carla_ros_scenario_runner_types/CMakeLists.txt b/carla_ros_scenario_runner_types/CMakeLists.txt new file mode 100644 index 00000000..8184efef --- /dev/null +++ b/carla_ros_scenario_runner_types/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 2.8.3) +project(carla_ros_scenario_runner_types) + +find_package(catkin REQUIRED COMPONENTS + message_generation + geometry_msgs +) + +add_service_files(DIRECTORY srv FILES + ExecuteScenario.srv +) + +add_message_files(DIRECTORY msg FILES + CarlaScenario.msg + CarlaScenarioList.msg + CarlaScenarioRunnerStatus.msg +) + +generate_messages(DEPENDENCIES geometry_msgs) + +catkin_package() diff --git a/carla_ros_scenario_runner_types/msg/CarlaScenario.msg b/carla_ros_scenario_runner_types/msg/CarlaScenario.msg new file mode 100644 index 00000000..bdb90fa3 --- /dev/null +++ b/carla_ros_scenario_runner_types/msg/CarlaScenario.msg @@ -0,0 +1,11 @@ +# +# Copyright (c) 2020 Intel Corporation. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . +# +string name +string scenario_file +geometry_msgs/Pose destination +float64 target_speed + diff --git a/carla_ros_scenario_runner_types/msg/CarlaScenarioList.msg b/carla_ros_scenario_runner_types/msg/CarlaScenarioList.msg new file mode 100644 index 00000000..8b2df049 --- /dev/null +++ b/carla_ros_scenario_runner_types/msg/CarlaScenarioList.msg @@ -0,0 +1,8 @@ +# +# Copyright (c) 2020 Intel Corporation. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . +# +CarlaScenario[] scenarios + diff --git a/carla_ros_scenario_runner_types/msg/CarlaScenarioRunnerStatus.msg b/carla_ros_scenario_runner_types/msg/CarlaScenarioRunnerStatus.msg new file mode 100644 index 00000000..cf0039e3 --- /dev/null +++ b/carla_ros_scenario_runner_types/msg/CarlaScenarioRunnerStatus.msg @@ -0,0 +1,14 @@ +# +# Copyright (c) 2020 Intel Corporation. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . +# +uint8 STOPPED = 0 +uint8 STARTING = 1 +uint8 RUNNING = 2 +uint8 SHUTTINGDOWN = 3 +uint8 ERROR = 4 + +uint8 status + diff --git a/carla_ros_scenario_runner_types/package.xml b/carla_ros_scenario_runner_types/package.xml new file mode 100644 index 00000000..5df0ef5a --- /dev/null +++ b/carla_ros_scenario_runner_types/package.xml @@ -0,0 +1,14 @@ + + + carla_ros_scenario_runner_types + 0.1.0 + The carla_ros_scenario_runner_types package + CARLA Simulator Team + MIT + catkin + message_generation + geometry_msgs + message_runtime + geometry_msgs + + diff --git a/carla_ros_scenario_runner_types/srv/ExecuteScenario.srv b/carla_ros_scenario_runner_types/srv/ExecuteScenario.srv new file mode 100644 index 00000000..d342a73f --- /dev/null +++ b/carla_ros_scenario_runner_types/srv/ExecuteScenario.srv @@ -0,0 +1,10 @@ +# +# Copyright (c) 2020 Intel Corporation. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . +# +CarlaScenario scenario +--- +bool result + diff --git a/carla_spectator_camera/src/carla_spectator_camera/carla_spectator_camera.py b/carla_spectator_camera/src/carla_spectator_camera/carla_spectator_camera.py index 7877514c..4d1c3201 100755 --- a/carla_spectator_camera/src/carla_spectator_camera/carla_spectator_camera.py +++ b/carla_spectator_camera/src/carla_spectator_camera/carla_spectator_camera.py @@ -24,6 +24,7 @@ # -- CarlaSpectatorCamera ------------------------------------------------------------ # ============================================================================== + class CarlaSpectatorCamera(object): """ Spawns a camera, attached to an ego vehicle. @@ -71,7 +72,7 @@ def get_camera_transform(self): return None if self.pose.header.frame_id != self.role_name: rospy.logwarn("Unsupported frame received. Supported {}, received {}".format( - self.role_name, self.pose.header.frame_id)) + self.role_name, self.pose.header.frame_id)) return None sensor_location = carla.Location(x=self.pose.pose.position.x, y=-self.pose.pose.position.y, @@ -83,7 +84,7 @@ def get_camera_transform(self): self.pose.pose.orientation.w ) roll, pitch, yaw = euler_from_quaternion(quaternion) - #rotate to CARLA + # rotate to CARLA sensor_rotation = carla.Rotation(pitch=math.degrees(roll)-90, roll=math.degrees(pitch), yaw=-math.degrees(yaw)-90) @@ -98,9 +99,9 @@ def create_camera(self, ego_actor): bp = bp_library.find("sensor.camera.rgb") bp.set_attribute('role_name', "spectator_view") rospy.loginfo("Creating camera with resolution {}x{}, fov {}".format( - self.camera_resolution_x, - self.camera_resolution_y, - self.camera_fov)) + self.camera_resolution_x, + self.camera_resolution_y, + self.camera_fov)) bp.set_attribute('image_size_x', str(self.camera_resolution_x)) bp.set_attribute('image_size_y', str(self.camera_resolution_y)) bp.set_attribute('fov', str(self.camera_fov)) @@ -165,7 +166,7 @@ def run(self): self.world = client.get_world() try: - r = rospy.Rate(10) # 10hz + r = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): self.find_ego_vehicle_actor() r.sleep() diff --git a/carla_twist_to_control/README.md b/carla_twist_to_control/README.md new file mode 100644 index 00000000..4fcfd7d0 --- /dev/null +++ b/carla_twist_to_control/README.md @@ -0,0 +1,17 @@ +# Twist to Vehicle Control conversion + +This node converts a [geometry_msgs.Twist](http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html) to [carla_msgs.CarlaEgoVehicleControl](../carla_msgs/msg/CarlaEgoVehicleControl.msg) + + +## Subscriptions + +| Topic | Type | Description | +| ---------------------------------- | ------------------- | --------------------------- | +| `/carla//vehicle_info` | [carla_msgs.CarlaEgoVehicleInfo](../carla_msgs/msg/CarlaEgoVehicleInfo.msg) | Ego vehicle info, to receive max steering angle | +| `/carla//twist` | [geometry_msgs.Twist](http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html) | Twist to convert | + +## Publications + +| Topic | Type | Description | +| ---------------------------------- | ------------------- | --------------------------- | +| `/carla//vehicle_control_cmd` | [carla_msgs.CarlaEgoVehicleControl](../carla_msgs/msg/CarlaEgoVehicleControl.msg) | Converted vehicle control command | diff --git a/carla_twist_to_control/src/carla_twist_to_control/carla_twist_to_control.py b/carla_twist_to_control/src/carla_twist_to_control/carla_twist_to_control.py index cb9bae89..dea8861a 100755 --- a/carla_twist_to_control/src/carla_twist_to_control/carla_twist_to_control.py +++ b/carla_twist_to_control/src/carla_twist_to_control/carla_twist_to_control.py @@ -14,7 +14,8 @@ from geometry_msgs.msg import Twist from carla_msgs.msg import CarlaEgoVehicleControl, CarlaEgoVehicleInfo -class TwistToVehicleControl(object): + +class TwistToVehicleControl(object): # pylint: disable=too-few-public-methods """ receive geometry_nav_msgs::Twist and publish carla_msgs::CarlaEgoVehicleControl @@ -30,11 +31,11 @@ def __init__(self, role_name): rospy.loginfo("Wait for vehicle info...") vehicle_info = rospy.wait_for_message("/carla/{}/vehicle_info".format(role_name), CarlaEgoVehicleInfo) - if not vehicle_info.wheels: + if not vehicle_info.wheels: # pylint: disable=no-member rospy.logerr("Cannot determine max steering angle: Vehicle has no wheels.") sys.exit(1) - - self.max_steering_angle = vehicle_info.wheels[0].max_steer_angle + + self.max_steering_angle = vehicle_info.wheels[0].max_steer_angle # pylint: disable=no-member if not self.max_steering_angle: rospy.logerr("Cannot determine max steering angle: Value is %s", self.max_steering_angle) @@ -53,30 +54,28 @@ def twist_received(self, twist): """ control = CarlaEgoVehicleControl() if twist == Twist(): - #stop + # stop control.throttle = 0. control.brake = 1. control.steer = 0. else: if twist.linear.x > 0: - control.throttle = min(TwistToVehicleControl.MAX_LON_ACCELERATION, twist.linear.x)/ \ - TwistToVehicleControl.MAX_LON_ACCELERATION + control.throttle = min(TwistToVehicleControl.MAX_LON_ACCELERATION, + twist.linear.x) / TwistToVehicleControl.MAX_LON_ACCELERATION else: control.reverse = True - control.throttle = max(-TwistToVehicleControl.MAX_LON_ACCELERATION, twist.linear.x)/ \ - -TwistToVehicleControl.MAX_LON_ACCELERATION + control.throttle = max(-TwistToVehicleControl.MAX_LON_ACCELERATION, + twist.linear.x) / -TwistToVehicleControl.MAX_LON_ACCELERATION if twist.angular.z > 0: - control.steer = -min(self.max_steering_angle, twist.angular.z)/ \ - self.max_steering_angle + control.steer = -min(self.max_steering_angle, twist.angular.z) / \ + self.max_steering_angle else: - control.steer = -max(-self.max_steering_angle, twist.angular.z)/ \ - self.max_steering_angle - #invert steer for reverse case - if control.reverse: - control.steer = -control.steer + control.steer = -max(-self.max_steering_angle, twist.angular.z) / \ + self.max_steering_angle self.pub.publish(control) + def main(): """ main function @@ -93,6 +92,6 @@ def main(): del twist_to_vehicle_control rospy.loginfo("Done") + if __name__ == "__main__": main() - diff --git a/carla_waypoint_publisher/README.md b/carla_waypoint_publisher/README.md index 7419a7ce..a2b756dc 100644 --- a/carla_waypoint_publisher/README.md +++ b/carla_waypoint_publisher/README.md @@ -13,7 +13,7 @@ Additionally, services are provided to query CARLA waypoints. As the waypoint publisher requires some Carla PythonAPI functionality that is not part of the python egg-file, you have to extend your PYTHONPATH. - export PYTHONPATH=$PYTHONPATH:/PythonAPI/carla-.egg:/PythonAPI/carla/ + export PYTHONPATH=$PYTHONPATH:/PythonAPI/carla/dist/carla-.egg:/PythonAPI/carla/ To run it: @@ -40,5 +40,5 @@ The calculated route is published: | Service | Description | Type | | ----------------------------------------------------------- | ----------- | -------------------------------------------------------------------- | -| `/carla_waypoint_publisher//get_waypoint` | Get the waypoint for a specific location | [carla_waypoint_types.GetWaypoint](../carla_waypoint_types/srv/GetWaypoint.msg) | -| `/carla_waypoint_publisher//get_actor_waypoint` | Get the waypoint for the ego vehicle | [carla_waypoint_types.GetActorWaypoint](../carla_waypoint_types/srv/GetActorWaypoint.msg) | +| `/carla_waypoint_publisher//get_waypoint` | Get the waypoint for a specific location | [carla_waypoint_types.GetWaypoint](../carla_waypoint_types/srv/GetWaypoint.srv) | +| `/carla_waypoint_publisher//get_actor_waypoint` | Get the waypoint for an actor id | [carla_waypoint_types.GetActorWaypoint](../carla_waypoint_types/srv/GetActorWaypoint.srv) | diff --git a/carla_waypoint_publisher/src/carla_waypoint_publisher/carla_waypoint_publisher.py b/carla_waypoint_publisher/src/carla_waypoint_publisher/carla_waypoint_publisher.py index 915bc708..95191278 100755 --- a/carla_waypoint_publisher/src/carla_waypoint_publisher/carla_waypoint_publisher.py +++ b/carla_waypoint_publisher/src/carla_waypoint_publisher/carla_waypoint_publisher.py @@ -110,13 +110,16 @@ def get_waypoint(self, req): #rospy.logwarn("Get waypoint {}".format(response.waypoint.pose.position)) return response - def get_actor_waypoint(self, _): + def get_actor_waypoint(self, req): """ Convenience method to get the waypoint for an actor """ + rospy.loginfo("get_actor_waypoint(): Get waypoint of actor {}".format(req.id)) + actor = self.world.get_actors().find(req.id) + response = GetActorWaypointResponse() - if self.ego_vehicle: - carla_waypoint = self.map.get_waypoint(self.ego_vehicle.get_location()) + if actor: + carla_waypoint = self.map.get_waypoint(actor.get_location()) response.waypoint.pose.position.x = carla_waypoint.transform.location.x response.waypoint.pose.position.y = -carla_waypoint.transform.location.y response.waypoint.pose.position.z = carla_waypoint.transform.location.z @@ -125,7 +128,7 @@ def get_actor_waypoint(self, _): response.waypoint.section_id = carla_waypoint.section_id response.waypoint.lane_id = carla_waypoint.lane_id else: - rospy.logwarn("get_actor_waypoint(): Ego vehicle not valid.") + rospy.logwarn("get_actor_waypoint(): Actor {} not valid.".format(req.id)) return response def on_goal(self, goal): diff --git a/carla_waypoint_types/srv/GetActorWaypoint.srv b/carla_waypoint_types/srv/GetActorWaypoint.srv index f7e7dad5..7b9100db 100644 --- a/carla_waypoint_types/srv/GetActorWaypoint.srv +++ b/carla_waypoint_types/srv/GetActorWaypoint.srv @@ -4,5 +4,6 @@ # This work is licensed under the terms of the MIT license. # For a copy, see . # +uint32 id --- carla_waypoint_types/CarlaWaypoint waypoint \ No newline at end of file diff --git a/check.sh b/check.sh index dcf47bb6..b16ec844 100755 --- a/check.sh +++ b/check.sh @@ -4,5 +4,10 @@ autopep8 carla_ackermann_control/src/carla_ackermann_control/*.py --in-place --m autopep8 carla_ego_vehicle/src/carla_ego_vehicle/*.py --in-place --max-line-length=100 autopep8 carla_waypoint_publisher/src/carla_waypoint_publisher/*.py --in-place --max-line-length=100 autopep8 rqt_carla_control/src/rqt_carla_control/*.py --in-place --max-line-length=100 +autopep8 carla_ad_agent/src/carla_ad_agent/*.py --in-place --max-line-length=100 +autopep8 carla_ros_scenario_runner/src/carla_ros_scenario_runner/*.py --in-place --max-line-length=100 +autopep8 carla_twist_to_control/src/carla_twist_to_control/*.py --in-place --max-line-length=100 +autopep8 carla_spectator_camera/src/carla_spectator_camera/*.py --in-place --max-line-length=100 +autopep8 carla_infrastructure/src/carla_infrastructure/*.py --in-place --max-line-length=100 -pylint --rcfile=.pylintrc carla_ackermann_control/src/carla_ackermann_control/ carla_ros_bridge/src/carla_ros_bridge/ carla_ego_vehicle/src/carla_ego_vehicle/ carla_waypoint_publisher/src/carla_waypoint_publisher/ rqt_carla_control/src/rqt_carla_control/ +pylint --rcfile=.pylintrc carla_ackermann_control/src/carla_ackermann_control/ carla_ros_bridge/src/carla_ros_bridge/ carla_ego_vehicle/src/carla_ego_vehicle/ carla_waypoint_publisher/src/carla_waypoint_publisher/ rqt_carla_control/src/rqt_carla_control/ carla_ad_agent/src/carla_ad_agent/ carla_ros_scenario_runner/src/carla_ros_scenario_runner/ carla_twist_to_control/src/carla_twist_to_control/ carla_spectator_camera/src/carla_spectator_camera/ carla_infrastructure/src/carla_infrastructure/ diff --git a/docs/images/rviz_carla_plugin.png b/docs/images/rviz_carla_plugin.png new file mode 100644 index 00000000..f7e8208d Binary files /dev/null and b/docs/images/rviz_carla_plugin.png differ diff --git a/rviz_carla_plugin/CMakeLists.txt b/rviz_carla_plugin/CMakeLists.txt new file mode 100644 index 00000000..ba6c616d --- /dev/null +++ b/rviz_carla_plugin/CMakeLists.txt @@ -0,0 +1,48 @@ +cmake_minimum_required(VERSION 2.8.3) +project(rviz_carla_plugin) + +set(CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_STANDARD 14) + +find_package(catkin REQUIRED COMPONENTS + rviz + carla_msgs + nav_msgs + carla_ros_scenario_runner_types +) +catkin_package() + +include_directories(${catkin_INCLUDE_DIRS}) +link_directories(${catkin_LIBRARY_DIRS}) + +set(CMAKE_AUTOMOC ON) + +message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") +find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) + +set(SRC_FILES + src/drive_widget.cpp + src/indicator_widget.cpp + src/carla_control_panel.cpp +) + +qt5_add_resources(SRC_FILES rviz_carla_plugin.qrc) + +add_library(${PROJECT_NAME} ${SRC_FILES}) + +target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES}) + +install(TARGETS + ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(FILES + plugin_description.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +install(DIRECTORY icons/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/icons) diff --git a/rviz_carla_plugin/README.md b/rviz_carla_plugin/README.md new file mode 100644 index 00000000..c1db4887 --- /dev/null +++ b/rviz_carla_plugin/README.md @@ -0,0 +1,64 @@ +# RVIZ CARLA Control + +A [RVIZ](http://wiki.ros.org/rviz) plugin to visualize/control CARLA. + +![CARLA AD Demo](../docs/images/rviz_carla_plugin.png "rviz carla plugin") + +This plugin is expecting a ego vehicle named `ego_vehicle`. + +## Features + +### Provide the RVIZ view pose to other nodes + +In combination with [carla_spectator_camera](../carla_spectator_camera), this allows visually moving around in the CARLA world. + +Currently, it is limited to a camera attached to the ego-vehicle. Please set the target frame of the "Current View" to `ego_vehicle`. + +### Visualize the current ego vehicle state + +The current vehicle state is visualized: + +- Vehicle Control +- Position + +### Allows manually overriding the ego vehicle vehicle control + +By using the drive-widget from the [RVIZ Visualization Tutorials](https://github.com/ros-visualization/visualization_tutorials) and a [node to convert from twist to vehicle control](../carla_twist_to_control) it is possible to steer the ego vehicle by mouse. + +### Execute a scenario + +By using [carla_ros_scenario_runner](../carla_ros_scenario_runner), it is possible to trigger scenarios from within RVIZ. + + +### Play/Pause the simulation (if started in synchronous mode) + +Similar to the [rqt CARLA plugin](../rqt_carla_plugin), it's possible to control the CARLA world, if synchronous mode is active. + + + +## Topics + +### Subscriptions + +| Topic | Type | Description | +| ------------------- | ------------------------- | ------------------------------------------------------ | +| `/carla/status` | [carla_msgs.CarlaStatus](../carla_msgs/msg/CarlaStatus.msg) | Read the status of CARLA, to enable/disable the UI | +| `/carla/ego_vehicle/vehicle_status` | [carla_msgs.CarlaEgoVehicleStatus](../carla_msgs/msg/CarlaEgoVehicleStatus.msg) | To display the current state of the ego vehicle | +| `/carla/ego_vehicle/odometry` | [nav_msgs.Odometry](http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html) | To display the current pose of the ego vehicle | +| `/scenario_runner/status` | [carla_ros_scenario_runner_types.CarlaScenarioRunnerStatus](../carla_ros_scenario_runner_types/msg/CarlaScenarioRunnerStatus.msg) | To visualize the scenario runner status | +| `/carla/available_scenarios` | [carla_ros_scenario_runner_types.CarlaScenarioList](../carla_ros_scenario_runner_types/msg/CarlaScenarioList.msg) | For providing a list of scenarios to execute (disabled in combo box) | + +### Publications + +| Topic | Type | Description | +| ------------------- | ------------------------- | ------------------------------------------------------ | +| `/carla/control` | [carla_msgs.CarlaControl](../carla_msgs/msg/CarlaControl.msg) | Start/pause CARLA | +| `/carla/ego_vehicle/spectator_pose` | [geometry_msgs.PoseStamped](http://docs.ros.org/api/geometry_msgs/html/msg/PoseStamped.html) | Publish the current pose of the RVIZ camera view. | +| `/carla/ego_vehicle/vehicle_control_manual_override` | [std_msgs.Bool](http://docs.ros.org/api/std_msgs/html/msg/Bool.html) | To enable/disable the overriding of the vehicle control | +| `/carla/ego_vehicle/twist` | [geometry_msgs.Twist](http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html) | The twist command, created via mouse | + +### Service Client + +| Service | Type | Description | +| ----------------------------------------------------------- | ---- | ------------------------------------------------------------------- | +| `/scenario_runner/execute_scenario` | [carla_ros_scenario_runner_types.ExecuteScenario](../carla_ros_scenario_runner_types/srv/ExecuteScenario.srv) | Execute the selected scenario | diff --git a/rviz_carla_plugin/icons/classes/CarlaControl.png b/rviz_carla_plugin/icons/classes/CarlaControl.png new file mode 100644 index 00000000..4b659faf Binary files /dev/null and b/rviz_carla_plugin/icons/classes/CarlaControl.png differ diff --git a/rviz_carla_plugin/icons/pause.png b/rviz_carla_plugin/icons/pause.png new file mode 100644 index 00000000..f486019f Binary files /dev/null and b/rviz_carla_plugin/icons/pause.png differ diff --git a/rviz_carla_plugin/icons/play.png b/rviz_carla_plugin/icons/play.png new file mode 100644 index 00000000..07078775 Binary files /dev/null and b/rviz_carla_plugin/icons/play.png differ diff --git a/rviz_carla_plugin/icons/step_once.png b/rviz_carla_plugin/icons/step_once.png new file mode 100644 index 00000000..7e85487f Binary files /dev/null and b/rviz_carla_plugin/icons/step_once.png differ diff --git a/rviz_carla_plugin/package.xml b/rviz_carla_plugin/package.xml new file mode 100644 index 00000000..c1a0d791 --- /dev/null +++ b/rviz_carla_plugin/package.xml @@ -0,0 +1,26 @@ + + + rviz_carla_plugin + 0.0.1 + The carla_ros_bridge package + CARLA Simulator Team + MIT + catkin + qtbase5-dev + rviz + carla_msgs + nav_msgs + geometry_msgs + carla_ros_scenario_runner_types + carla_msgs + nav_msgs + geometry_msgs + carla_ros_scenario_runner_types + libqt5-core + libqt5-gui + libqt5-widgets + rviz + + + + diff --git a/rviz_carla_plugin/plugin_description.xml b/rviz_carla_plugin/plugin_description.xml new file mode 100644 index 00000000..42f236a3 --- /dev/null +++ b/rviz_carla_plugin/plugin_description.xml @@ -0,0 +1,9 @@ + + + + A panel widget for controlling carla nodes + + + diff --git a/rviz_carla_plugin/rviz_carla_plugin.qrc b/rviz_carla_plugin/rviz_carla_plugin.qrc new file mode 100644 index 00000000..1ebe3370 --- /dev/null +++ b/rviz_carla_plugin/rviz_carla_plugin.qrc @@ -0,0 +1,7 @@ + + + icons/play.png + icons/pause.png + icons/step_once.png + + \ No newline at end of file diff --git a/rviz_carla_plugin/src/carla_control_panel.cpp b/rviz_carla_plugin/src/carla_control_panel.cpp new file mode 100644 index 00000000..ce436946 --- /dev/null +++ b/rviz_carla_plugin/src/carla_control_panel.cpp @@ -0,0 +1,411 @@ +/* + * Copyright (c) 2020 Intel Corporation + * + * This work is licensed under the terms of the MIT license. + * For a copy, see . + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include "carla_control_panel.h" +#include "drive_widget.h" +#include "indicator_widget.h" + +namespace rviz_carla_plugin { + +CarlaControlPanel::CarlaControlPanel(QWidget *parent) + : rviz::Panel(parent) +{ + QVBoxLayout *layout = new QVBoxLayout; + QHBoxLayout *vehicleLayout = new QHBoxLayout; + + QFormLayout *egoCtrlStatusLayout = new QFormLayout; + + mThrottleBar = new QProgressBar(); + mThrottleBar->setRange(0, 100); + egoCtrlStatusLayout->addRow("Throttle", mThrottleBar); + mBrakeBar = new QProgressBar(); + mBrakeBar->setRange(0, 100); + egoCtrlStatusLayout->addRow("Brake", mBrakeBar); + mSteerBar = new QProgressBar(); + mSteerBar->setRange(-100, 100); + egoCtrlStatusLayout->addRow("Steer", mSteerBar); + vehicleLayout->addLayout(egoCtrlStatusLayout); + + QFormLayout *egoStatusLayout = new QFormLayout; + mPosLabel = new QLineEdit(); + mPosLabel->setDisabled(true); + egoStatusLayout->addRow("Position", mPosLabel); + + mSpeedLabel = new QLineEdit(); + mSpeedLabel->setDisabled(true); + egoStatusLayout->addRow("Speed", mSpeedLabel); + + mHeadingLabel = new QLineEdit(); + mHeadingLabel->setDisabled(true); + egoStatusLayout->addRow("Heading", mHeadingLabel); + + vehicleLayout->addLayout(egoStatusLayout); + + QVBoxLayout *egoCtrlLayout = new QVBoxLayout; + mOverrideVehicleControl = new QCheckBox("Manual control"); + mOverrideVehicleControl->setDisabled(true); + egoCtrlLayout->addWidget(mOverrideVehicleControl); + mDriveWidget = new DriveWidget; + mDriveWidget->setDisabled(true); + egoCtrlLayout->addWidget(mDriveWidget); + connect(mOverrideVehicleControl, SIGNAL(stateChanged(int)), this, SLOT(overrideVehicleControl(int))); + + vehicleLayout->addLayout(egoCtrlLayout); + + layout->addLayout(vehicleLayout); + + QFormLayout *carlaLayout = new QFormLayout; + + // row1 + QHBoxLayout *carlaRow1Layout = new QHBoxLayout; + + mScenarioSelection = new QComboBox(); + carlaRow1Layout->addWidget(mScenarioSelection, 10); + + mTriggerScenarioButton = new QPushButton("Execute"); + carlaRow1Layout->addWidget(mTriggerScenarioButton); + + mIndicatorWidget = new IndicatorWidget(); + carlaRow1Layout->addWidget(mIndicatorWidget); + connect(mTriggerScenarioButton, SIGNAL(released()), this, SLOT(executeScenario())); + + carlaLayout->addRow("Scenario Execution", carlaRow1Layout); + + QHBoxLayout *synchronous_layout = new QHBoxLayout; + QPixmap pixmap(":/icons/play.png"); + QIcon iconPlay(pixmap); + mPlayPauseButton = new QPushButton(iconPlay, ""); + synchronous_layout->addWidget(mPlayPauseButton); + connect(mPlayPauseButton, SIGNAL(released()), this, SLOT(carlaTogglePlayPause())); + + QPixmap pixmapStepOnce(":/icons/step_once.png"); + QIcon iconStepOnce(pixmapStepOnce); + mStepOnceButton = new QPushButton(iconStepOnce, ""); + connect(mStepOnceButton, SIGNAL(released()), this, SLOT(carlaStepOnce())); + + synchronous_layout->addWidget(mStepOnceButton); + carlaLayout->addRow("Carla Control", synchronous_layout); + + layout->addLayout(carlaLayout); + + setLayout(layout); + + QTimer *outputTimer = new QTimer(this); + connect(outputTimer, SIGNAL(timeout()), this, SLOT(sendVel())); + outputTimer->start(100); + + connect(mDriveWidget, SIGNAL(outputVelocity(float, float)), this, SLOT(setVel(float, float))); + mDriveWidget->setEnabled(false); + + setSimulationButtonStatus(false); + setScenarioRunnerStatus(false); + + mCarlaStatusSubscriber = mNodeHandle.subscribe("/carla/status", 1000, &CarlaControlPanel::carlaStatusChanged, this); + mCarlaControlPublisher = mNodeHandle.advertise("/carla/control", 10); + mEgoVehicleStatusSubscriber = mNodeHandle.subscribe( + "/carla/ego_vehicle/vehicle_status", 1000, &CarlaControlPanel::egoVehicleStatusChanged, this); + mEgoVehicleOdometrySubscriber + = mNodeHandle.subscribe("/carla/ego_vehicle/odometry", 1000, &CarlaControlPanel::egoVehicleOdometryChanged, this); + + mCameraPosePublisher + = mNodeHandle.advertise("carla/ego_vehicle/spectator_pose", 10, true); + + mEgoVehicleControlManualOverridePublisher + = mNodeHandle.advertise("/carla/ego_vehicle/vehicle_control_manual_override", 1, true); + + mExecuteScenarioClient + = mNodeHandle.serviceClient("/scenario_runner/execute_scenario"); + mScenarioRunnerStatusSubscriber + = mNodeHandle.subscribe("/scenario_runner/status", 10, &CarlaControlPanel::scenarioRunnerStatusChanged, this); + + mTwistPublisher = mNodeHandle.advertise("/carla/ego_vehicle/twist", 1); + + mScenarioSubscriber + = mNodeHandle.subscribe("/carla/available_scenarios", 1, &CarlaControlPanel::carlaScenariosChanged, this); + + // //initially set the camera + QTimer::singleShot(1000, this, SLOT(updateCameraPos())); +} + +void CarlaControlPanel::cameraPreRenderScene(Ogre::Camera *cam) +{ + double epsilon = 0.001; + if ((std::fabs(cam->getPosition().x - mCameraCurrentPosition.x) > epsilon) + || (std::fabs(cam->getPosition().y - mCameraCurrentPosition.y) > epsilon) + || (std::fabs(cam->getPosition().z - mCameraCurrentPosition.z) > epsilon) + || (std::fabs(cam->getOrientation().x - mCameraCurrentOrientation.x) > epsilon) + || (std::fabs(cam->getOrientation().y - mCameraCurrentOrientation.y) > epsilon) + || (std::fabs(cam->getOrientation().z - mCameraCurrentOrientation.z) > epsilon) + || (std::fabs(cam->getOrientation().w - mCameraCurrentOrientation.w) > epsilon)) + { + mCameraCurrentPosition = cam->getPosition(); + mCameraCurrentOrientation = cam->getOrientation(); + QTimer::singleShot(0, this, SLOT(updateCameraPos())); + } +} + +void CarlaControlPanel::updateCameraPos() +{ + auto frame = mViewController->subProp("Target Frame")->getValue(); + + geometry_msgs::PoseStamped pose; + pose.header.frame_id = frame.toString().toStdString(); + pose.header.stamp = ros::Time::now(); + pose.pose.position.x = mCameraCurrentPosition.x; + pose.pose.position.y = mCameraCurrentPosition.y; + pose.pose.position.z = mCameraCurrentPosition.z; + pose.pose.orientation.x = mCameraCurrentOrientation.x; + pose.pose.orientation.y = mCameraCurrentOrientation.y; + pose.pose.orientation.z = mCameraCurrentOrientation.z; + pose.pose.orientation.w = mCameraCurrentOrientation.w; + + mCameraPosePublisher.publish(pose); +} + +void CarlaControlPanel::onInitialize() +{ + currentViewControllerChanged(); + connect(vis_manager_->getViewManager(), SIGNAL(currentChanged()), this, SLOT(currentViewControllerChanged())); +} + +void CarlaControlPanel::currentViewControllerChanged() +{ + mViewController + = dynamic_cast(vis_manager_->getViewManager()->getCurrent()); + if (!mViewController) + { + ROS_ERROR("Invalid view controller!"); + return; + } + + auto camera = mViewController->getCamera(); + camera->addListener(this); +} + +void CarlaControlPanel::executeScenario() +{ + for (auto const &scenario : mCarlaScenarios->scenarios) + { + if (QString::fromStdString(scenario.name) == mScenarioSelection->currentText()) + { + carla_ros_scenario_runner_types::ExecuteScenario srv; + srv.request.scenario = scenario; + if (!mExecuteScenarioClient.call(srv)) + { + ROS_ERROR("Failed to call service executeScenario"); + mIndicatorWidget->setState(IndicatorWidget::State::Error); + } + break; + } + } +} + +void CarlaControlPanel::overrideVehicleControl(int state) +{ + std_msgs::Bool boolMsg; + if (state == Qt::Checked) + { + boolMsg.data = true; + mDriveWidget->setEnabled(true); + } + else + { + boolMsg.data = false; + mDriveWidget->setEnabled(false); + } + mEgoVehicleControlManualOverridePublisher.publish(boolMsg); +} + +void CarlaControlPanel::scenarioRunnerStatusChanged( + const carla_ros_scenario_runner_types::CarlaScenarioRunnerStatus::ConstPtr &msg) +{ + if (msg->status == carla_ros_scenario_runner_types::CarlaScenarioRunnerStatus::STOPPED) + { + mIndicatorWidget->setState(IndicatorWidget::State::Stopped); + } + else if (msg->status == carla_ros_scenario_runner_types::CarlaScenarioRunnerStatus::STARTING) + { + mIndicatorWidget->setState(IndicatorWidget::State::Starting); + } + else if (msg->status == carla_ros_scenario_runner_types::CarlaScenarioRunnerStatus::RUNNING) + { + mIndicatorWidget->setState(IndicatorWidget::State::Running); + } + else if (msg->status == carla_ros_scenario_runner_types::CarlaScenarioRunnerStatus::SHUTTINGDOWN) + { + mIndicatorWidget->setState(IndicatorWidget::State::ShuttingDown); + } + else + { + mIndicatorWidget->setState(IndicatorWidget::State::Error); + } +} + +void CarlaControlPanel::setScenarioRunnerStatus(bool active) +{ + mScenarioSelection->setEnabled(active); + mTriggerScenarioButton->setEnabled(active); + mIndicatorWidget->setEnabled(active); +} + +void CarlaControlPanel::carlaScenariosChanged(const carla_ros_scenario_runner_types::CarlaScenarioList::ConstPtr &msg) +{ + auto currentSelection = mScenarioSelection->currentText(); + mCarlaScenarios = msg; + mScenarioSelection->clear(); + int idx = 0; + for (auto const &scenario : msg->scenarios) + { + auto name = QString::fromStdString(scenario.name); + mScenarioSelection->addItem(name); + if (name == currentSelection) + { // switch to previously selected item + mScenarioSelection->setCurrentIndex(idx); + } + ++idx; + } + setScenarioRunnerStatus(mScenarioSelection->count() > 0); +} + +void CarlaControlPanel::egoVehicleStatusChanged(const carla_msgs::CarlaEgoVehicleStatus::ConstPtr &msg) +{ + mOverrideVehicleControl->setEnabled(true); + mSteerBar->setValue(msg->control.steer * 100); + mThrottleBar->setValue(msg->control.throttle * 100); + mBrakeBar->setValue(msg->control.brake * 100); + + std::stringstream speedStream; + speedStream << std::fixed << std::setprecision(2) << msg->velocity * 3.6; + mSpeedLabel->setText(speedStream.str().c_str()); +} + +void CarlaControlPanel::egoVehicleOdometryChanged(const nav_msgs::Odometry::ConstPtr &msg) +{ + std::stringstream posStream; + posStream << std::fixed << std::setprecision(2) << msg->pose.pose.position.x << ", " << msg->pose.pose.position.y; + mPosLabel->setText(posStream.str().c_str()); + + std::stringstream headingStream; + headingStream << std::fixed << std::setprecision(2) << tf::getYaw(msg->pose.pose.orientation); + mHeadingLabel->setText(headingStream.str().c_str()); +} + +void CarlaControlPanel::setSimulationButtonStatus(bool active) +{ + if (active) + { + mPlayPauseButton->setDisabled(false); + mPlayPauseButton->setToolTip("Play/Pause the CARLA world."); + mStepOnceButton->setDisabled(false); + mStepOnceButton->setToolTip("Execute on tick within the CARLA world."); + } + else + { + mPlayPauseButton->setDisabled(true); + mPlayPauseButton->setToolTip("Disabled due to CARLA running in non-synchronous mode."); + mStepOnceButton->setDisabled(true); + mStepOnceButton->setToolTip("Disabled due to CARLA running in non-synchronous mode."); + } +} + +void CarlaControlPanel::carlaStatusChanged(const carla_msgs::CarlaStatus::ConstPtr &msg) +{ + mCarlaStatus = msg; + setSimulationButtonStatus(mCarlaStatus->synchronous_mode); + + if (mCarlaStatus->synchronous_mode) + { + if (mCarlaStatus->synchronous_mode_running) + { + QPixmap pixmap(":/icons/pause.png"); + QIcon iconPause(pixmap); + mPlayPauseButton->setIcon(iconPause); + } + else + { + QPixmap pixmap(":/icons/play.png"); + QIcon iconPlay(pixmap); + mPlayPauseButton->setIcon(iconPlay); + } + } +} + +void CarlaControlPanel::carlaStepOnce() +{ + carla_msgs::CarlaControl ctrl; + ctrl.command = carla_msgs::CarlaControl::STEP_ONCE; + mCarlaControlPublisher.publish(ctrl); +} + +void CarlaControlPanel::carlaTogglePlayPause() +{ + if (mCarlaStatus) + { + carla_msgs::CarlaControl ctrl; + if (mCarlaStatus->synchronous_mode_running) + { + ctrl.command = carla_msgs::CarlaControl::PAUSE; + } + else + { + ctrl.command = carla_msgs::CarlaControl::PLAY; + } + mCarlaControlPublisher.publish(ctrl); + } +} + +void CarlaControlPanel::setVel(float linearVelocity, float angularVelocity) +{ + mLinearVelocity = linearVelocity; + mAngularVelocity = angularVelocity; +} + +void CarlaControlPanel::sendVel() +{ + if (ros::ok() && mTwistPublisher && (mOverrideVehicleControl->checkState() == Qt::Checked)) + { + geometry_msgs::Twist msg; + msg.linear.x = mLinearVelocity; + msg.linear.y = 0; + msg.linear.z = 0; + msg.angular.x = 0; + msg.angular.y = 0; + msg.angular.z = mAngularVelocity; + mTwistPublisher.publish(msg); + } +} + +} // end namespace rviz_carla_plugin + +#include +PLUGINLIB_EXPORT_CLASS(rviz_carla_plugin::CarlaControlPanel, rviz::Panel) diff --git a/rviz_carla_plugin/src/carla_control_panel.h b/rviz_carla_plugin/src/carla_control_panel.h new file mode 100644 index 00000000..646b96dd --- /dev/null +++ b/rviz_carla_plugin/src/carla_control_panel.h @@ -0,0 +1,104 @@ +/* + * Copyright (c) 2020 Intel Corporation + * + * This work is licensed under the terms of the MIT license. + * For a copy, see . + */ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +class QLineEdit; +class QPushButton; +class QProgressBar; +class QCheckBox; +class QComboBox; + +namespace rviz { +class ViewController; +class FramePositionTrackingViewController; +} + +namespace rviz_carla_plugin { + +class DriveWidget; +class IndicatorWidget; + +class CarlaControlPanel : public rviz::Panel, public Ogre::Camera::Listener +{ + Q_OBJECT +public: + CarlaControlPanel(QWidget *parent = 0); + +public Q_SLOTS: + void setVel(float linearVelocity, float angularVelocity); + +protected Q_SLOTS: + void sendVel(); + + void carlaStepOnce(); + void carlaTogglePlayPause(); + void overrideVehicleControl(int state); + void executeScenario(); + + void updateCameraPos(); + void currentViewControllerChanged(); + +protected: + virtual void cameraPreRenderScene(Ogre::Camera *cam) override; + + virtual void onInitialize() override; + void setSimulationButtonStatus(bool active); + void setScenarioRunnerStatus(bool active); + + void scenarioRunnerStatusChanged(const carla_ros_scenario_runner_types::CarlaScenarioRunnerStatus::ConstPtr &msg); + void carlaStatusChanged(const carla_msgs::CarlaStatus::ConstPtr &msg); + void egoVehicleStatusChanged(const carla_msgs::CarlaEgoVehicleStatus::ConstPtr &msg); + void egoVehicleOdometryChanged(const nav_msgs::Odometry::ConstPtr &msg); + void carlaScenariosChanged(const carla_ros_scenario_runner_types::CarlaScenarioList::ConstPtr &msg); + carla_msgs::CarlaStatus::ConstPtr mCarlaStatus{nullptr}; + + DriveWidget *mDriveWidget; + QPushButton *mTriggerScenarioButton; + QPushButton *mPlayPauseButton; + QPushButton *mStepOnceButton; + QProgressBar *mThrottleBar; + QProgressBar *mBrakeBar; + QProgressBar *mSteerBar; + QLineEdit *mPosLabel; + QLineEdit *mSpeedLabel; + QLineEdit *mHeadingLabel; + QCheckBox *mOverrideVehicleControl; + QComboBox *mScenarioSelection; + IndicatorWidget *mIndicatorWidget; + ros::Publisher mTwistPublisher; + ros::Publisher mCarlaControlPublisher; + ros::Publisher mEgoVehicleControlManualOverridePublisher; + ros::Subscriber mCarlaStatusSubscriber; + ros::Subscriber mEgoVehicleStatusSubscriber; + ros::Subscriber mEgoVehicleOdometrySubscriber; + ros::ServiceClient mExecuteScenarioClient; + ros::Subscriber mScenarioSubscriber; + ros::Subscriber mScenarioRunnerStatusSubscriber; + ros::Publisher mCameraPosePublisher; + + carla_ros_scenario_runner_types::CarlaScenarioList::ConstPtr mCarlaScenarios; + + ros::NodeHandle mNodeHandle; + + float mLinearVelocity{0.0}; + float mAngularVelocity{0.0}; + bool mVehicleControlManualOverride{false}; + rviz::FramePositionTrackingViewController *mViewController{nullptr}; + Ogre::Vector3 mCameraCurrentPosition; + Ogre::Quaternion mCameraCurrentOrientation; +}; + +} // end namespace rviz_carla_plugin diff --git a/rviz_carla_plugin/src/drive_widget.cpp b/rviz_carla_plugin/src/drive_widget.cpp new file mode 100644 index 00000000..2de48ac2 --- /dev/null +++ b/rviz_carla_plugin/src/drive_widget.cpp @@ -0,0 +1,249 @@ +/* + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#include +#include + +#include "drive_widget.h" + +namespace rviz_carla_plugin { + +// BEGIN_TUTORIAL +// The DriveWidget constructor does the normal Qt thing of +// passing the parent widget to the superclass constructor, then +// initializing the member variables. +DriveWidget::DriveWidget(QWidget *parent) + : QWidget(parent) + , linear_velocity_(0) + , angular_velocity_(0) + , linear_scale_(10) + , angular_scale_(.2) +{ +} + +// This paintEvent() is complex because of the drawing of the two +// arc-arrows representing wheel motion. It is not particularly +// relevant to learning how to make an RViz plugin, so I will kind of +// skim it. +void DriveWidget::paintEvent(QPaintEvent *event) +{ + // The background color and crosshair lines are drawn differently + // depending on whether this widget is enabled or not. This gives a + // nice visual indication of whether the control is "live". + QColor background; + QColor crosshair; + if (isEnabled()) + { + background = Qt::white; + crosshair = Qt::black; + } + else + { + background = Qt::lightGray; + crosshair = Qt::darkGray; + } + + // The main visual is a square, centered in the widget's area. Here + // we compute the size of the square and the horizontal and vertical + // offsets of it. + int w = width(); + int h = height(); + int size = ((w > h) ? h : w) - 1; + int hpad = (w - size) / 2; + int vpad = (h - size) / 2; + + QPainter painter(this); + painter.setBrush(background); + painter.setPen(crosshair); + + // Draw the background square. + painter.drawRect(QRect(hpad, vpad, size, size)); + + // Draw a cross-hair inside the square. + painter.drawLine(hpad, height() / 2, hpad + size, height() / 2); + painter.drawLine(width() / 2, vpad, width() / 2, vpad + size); + + // If the widget is enabled and the velocities are not zero, draw + // some sweet green arrows showing possible paths that the wheels of + // a diff-drive robot would take if it stayed at these velocities. + if (isEnabled() && (angular_velocity_ != 0 || linear_velocity_ != 0)) + { + QPen arrow; + arrow.setWidth(size / 20); + arrow.setColor(Qt::green); + arrow.setCapStyle(Qt::RoundCap); + arrow.setJoinStyle(Qt::RoundJoin); + painter.setPen(arrow); + + // This code steps along a central arc defined by the linear and + // angular velocites. At each step, it computes where the left + // and right wheels would be and collects the resulting points + // in the left_track and right_track arrays. + const int step_count = 100; + QPointF left_track[step_count]; + QPointF right_track[step_count]; + + float half_track_width = size / 4.0; + + float cx = w / 2; + float cy = h / 2; + left_track[0].setX(cx - half_track_width); + left_track[0].setY(cy); + right_track[0].setX(cx + half_track_width); + right_track[0].setY(cy); + float angle = M_PI / 2; + float delta_angle = angular_velocity_ / step_count; + float step_dist = linear_velocity_ * size / 2 / linear_scale_ / step_count; + for (int step = 1; step < step_count; step++) + { + angle += delta_angle / 2; + float next_cx = cx + step_dist * cosf(angle); + float next_cy = cy - step_dist * sinf(angle); + angle += delta_angle / 2; + + left_track[step].setX(next_cx + half_track_width * cosf(angle + M_PI / 2)); + left_track[step].setY(next_cy - half_track_width * sinf(angle + M_PI / 2)); + right_track[step].setX(next_cx + half_track_width * cosf(angle - M_PI / 2)); + right_track[step].setY(next_cy - half_track_width * sinf(angle - M_PI / 2)); + + cx = next_cx; + cy = next_cy; + } + // Now the track arrays are filled, so stroke each with a fat green line. + painter.drawPolyline(left_track, step_count); + painter.drawPolyline(right_track, step_count); + + // Here we decide which direction each arrowhead will point + // (forward or backward). This works by comparing the arc length + // travelled by the center in one step (step_dist) with the arc + // length travelled by the wheel (half_track_width * delta_angle). + int left_arrow_dir = (-step_dist + half_track_width * delta_angle > 0); + int right_arrow_dir = (-step_dist - half_track_width * delta_angle > 0); + + // Use MiterJoin for the arrowheads so we get a nice sharp point. + arrow.setJoinStyle(Qt::MiterJoin); + painter.setPen(arrow); + + // Compute and draw polylines for each arrowhead. This code could + // probably be more elegant. + float head_len = size / 8.0; + QPointF arrow_head[3]; + float x, y; + if (fabsf(-step_dist + half_track_width * delta_angle) > .01) + { + x = left_track[step_count - 1].x(); + y = left_track[step_count - 1].y(); + arrow_head[0].setX(x + head_len * cosf(angle + 3 * M_PI / 4 + left_arrow_dir * M_PI)); + arrow_head[0].setY(y - head_len * sinf(angle + 3 * M_PI / 4 + left_arrow_dir * M_PI)); + arrow_head[1].setX(x); + arrow_head[1].setY(y); + arrow_head[2].setX(x + head_len * cosf(angle - 3 * M_PI / 4 + left_arrow_dir * M_PI)); + arrow_head[2].setY(y - head_len * sinf(angle - 3 * M_PI / 4 + left_arrow_dir * M_PI)); + painter.drawPolyline(arrow_head, 3); + } + if (fabsf(-step_dist - half_track_width * delta_angle) > .01) + { + x = right_track[step_count - 1].x(); + y = right_track[step_count - 1].y(); + arrow_head[0].setX(x + head_len * cosf(angle + 3 * M_PI / 4 + right_arrow_dir * M_PI)); + arrow_head[0].setY(y - head_len * sinf(angle + 3 * M_PI / 4 + right_arrow_dir * M_PI)); + arrow_head[1].setX(x); + arrow_head[1].setY(y); + arrow_head[2].setX(x + head_len * cosf(angle - 3 * M_PI / 4 + right_arrow_dir * M_PI)); + arrow_head[2].setY(y - head_len * sinf(angle - 3 * M_PI / 4 + right_arrow_dir * M_PI)); + painter.drawPolyline(arrow_head, 3); + } + } +} + +// Every mouse move event received here sends a velocity because Qt +// only sends us mouse move events if there was previously a +// mouse-press event while in the widget. +void DriveWidget::mouseMoveEvent(QMouseEvent *event) +{ + sendVelocitiesFromMouse(event->x(), event->y(), width(), height()); +} + +// Mouse-press events should send the velocities too, of course. +void DriveWidget::mousePressEvent(QMouseEvent *event) +{ + sendVelocitiesFromMouse(event->x(), event->y(), width(), height()); +} + +// When the mouse leaves the widget but the button is still held down, +// we don't get the leaveEvent() because the mouse is "grabbed" (by +// default from Qt). However, when the mouse drags out of the widget +// and then other buttons are pressed (or possibly other +// window-manager things happen), we will get a leaveEvent() but not a +// mouseReleaseEvent(). Without catching this event you can have a +// robot stuck "on" without the user controlling it. +void DriveWidget::leaveEvent(QEvent *event) +{ + stop(); +} + +// The ordinary way to stop: let go of the mouse button. +void DriveWidget::mouseReleaseEvent(QMouseEvent *event) +{ + stop(); +} + +// Compute and emit linear and angular velocities based on Y and X +// mouse positions relative to the central square. +void DriveWidget::sendVelocitiesFromMouse(int x, int y, int width, int height) +{ + int size = ((width > height) ? height : width); + int hpad = (width - size) / 2; + int vpad = (height - size) / 2; + + linear_velocity_ = (1.0 - float(y - vpad) / float(size / 2)) * linear_scale_; + angular_velocity_ = (1.0 - float(x - hpad) / float(size / 2)) * angular_scale_; + Q_EMIT outputVelocity(linear_velocity_, angular_velocity_); + + // update() is a QWidget function which schedules this widget to be + // repainted the next time through the main event loop. We need + // this because the velocities have just changed, so the arrows need + // to be redrawn to match. + update(); +} + +// How to stop: emit velocities of 0! +void DriveWidget::stop() +{ + linear_velocity_ = 0; + angular_velocity_ = 0; + Q_EMIT outputVelocity(linear_velocity_, angular_velocity_); + update(); +} +// END_TUTORIAL + +} // end namespace rviz_carla_plugin diff --git a/rviz_carla_plugin/src/drive_widget.h b/rviz_carla_plugin/src/drive_widget.h new file mode 100644 index 00000000..e0a6c7d0 --- /dev/null +++ b/rviz_carla_plugin/src/drive_widget.h @@ -0,0 +1,91 @@ +/* + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef DRIVE_WIDGET_H +#define DRIVE_WIDGET_H + +#include + +namespace rviz_carla_plugin { + +// BEGIN_TUTORIAL +// DriveWidget implements a control which translates mouse Y values +// into linear velocities and mouse X values into angular velocities. +// +// For maximum reusability, this class is only responsible for user +// interaction and display inside its widget. It does not make any +// ROS or RViz calls. It communicates its data to the outside just +// via Qt signals. +class DriveWidget : public QWidget +{ + Q_OBJECT +public: + // This class is not instantiated by pluginlib::ClassLoader, so the + // constructor has no restrictions. + DriveWidget(QWidget *parent = 0); + + // We override QWidget::paintEvent() to do custom painting. + virtual void paintEvent(QPaintEvent *event); + + // We override the mouse events and leaveEvent() to keep track of + // what the mouse is doing. + virtual void mouseMoveEvent(QMouseEvent *event); + virtual void mousePressEvent(QMouseEvent *event); + virtual void mouseReleaseEvent(QMouseEvent *event); + virtual void leaveEvent(QEvent *event); + + // Override sizeHint() to give the layout managers some idea of a + // good size for this. + virtual QSize sizeHint() const + { + return QSize(150, 150); + } + + // We emit outputVelocity() whenever it changes. +Q_SIGNALS: + void outputVelocity(float linear, float angular); + + // mouseMoveEvent() and mousePressEvent() need the same math to + // figure the velocities, so I put that in here. +protected: + void sendVelocitiesFromMouse(int x, int y, int width, int height); + + // A function to emit zero velocity. + void stop(); + + // Finally the member variables: + float linear_velocity_; // In m/s + float angular_velocity_; // In radians/s + float linear_scale_; // In m/s + float angular_scale_; // In radians/s +}; +// END_TUTORIAL + +} // end namespace rviz_carla_plugin + +#endif // DRIVE_WIDGET_H diff --git a/rviz_carla_plugin/src/indicator_widget.cpp b/rviz_carla_plugin/src/indicator_widget.cpp new file mode 100644 index 00000000..ae160a42 --- /dev/null +++ b/rviz_carla_plugin/src/indicator_widget.cpp @@ -0,0 +1,53 @@ +/* + * Copyright (c) 2020 Intel Corporation + * + * This work is licensed under the terms of the MIT license. + * For a copy, see . + */ +#include "indicator_widget.h" +#include + +namespace rviz_carla_plugin { + +IndicatorWidget::IndicatorWidget(QWidget *parent) + : QWidget(parent) + , mCurrentState(IndicatorWidget::State::Stopped) +{ + setFixedSize(18, 18); +} + +void IndicatorWidget::paintEvent(QPaintEvent *event) +{ + QPainter painter(this); + painter.setPen(Qt::darkGray); + if (mCurrentState == IndicatorWidget::State::Stopped) + { + painter.setBrush(QBrush(Qt::lightGray, Qt::SolidPattern)); + } + else if (mCurrentState == IndicatorWidget::State::Running) + { + painter.setBrush(QBrush(Qt::green, Qt::SolidPattern)); + } + else if (mCurrentState == IndicatorWidget::State::Error) + { + painter.setBrush(QBrush(Qt::red, Qt::SolidPattern)); + } + else if ((mCurrentState == IndicatorWidget::State::Starting) + || (mCurrentState == IndicatorWidget::State::ShuttingDown)) + { + painter.setBrush(QBrush(Qt::yellow, Qt::SolidPattern)); + } + else + { + painter.setBrush(QBrush(Qt::black, Qt::SolidPattern)); + } + painter.drawEllipse(1, 1, 16, 16); +} + +void IndicatorWidget::setState(IndicatorWidget::State state) +{ + mCurrentState = state; + repaint(); +} + +} // end namespace rviz_carla_plugin diff --git a/rviz_carla_plugin/src/indicator_widget.h b/rviz_carla_plugin/src/indicator_widget.h new file mode 100644 index 00000000..2d5ea19f --- /dev/null +++ b/rviz_carla_plugin/src/indicator_widget.h @@ -0,0 +1,36 @@ +/* + * Copyright (c) 2020 Intel Corporation + * + * This work is licensed under the terms of the MIT license. + * For a copy, see . + */ +#pragma once + +#include + +namespace rviz_carla_plugin { + +class IndicatorWidget : public QWidget +{ + Q_OBJECT +public: + enum class State + { + Stopped, + Starting, + Running, + ShuttingDown, + Error + }; + + IndicatorWidget(QWidget *parent = 0); + + virtual void paintEvent(QPaintEvent *event) override; + + void setState(IndicatorWidget::State state); + +private: + State mCurrentState{State::Stopped}; +}; + +} // end namespace rviz_carla_plugin