コード例 #1
0
class RosVehicleControl(BasicControl):
    def __init__(self, actor, args=None):
        super(RosVehicleControl, self).__init__(actor)
        ros_init(args=None)

        self._carla_actor = actor
        self._role_name = actor.attributes["role_name"]
        if not self._role_name:
            if ROS_VERSION == 1:
                rospy.logerr("Invalid role_name!")
            elif ROS_VERSION == 2:
                logging.get_logger("pre_logger").error("Invalid role_name!")

        self._path_topic_name = "waypoints"
        if "path_topic_name" in args:
            self._path_topic_name = args["path_topic_name"]

        self.node = CompatibleNode('ros_agent_{}'.format(self._role_name))

        self._current_target_speed = None
        self._current_path = None
        self.controller_launch = None

        self._target_speed_publisher = self.node.new_publisher(
            Float64, "/carla/{}/target_speed".format(self._role_name),
            QoSProfile(depth=10, durability=latch_on))
        self.node.loginfo(
            "Publishing target_speed on /carla/{}/target_speed".format(
                self._role_name))

        self._path_publisher = self.node.new_publisher(
            Path, "/carla/{}/{}".format(self._role_name,
                                        self._path_topic_name),
            QoSProfile(depth=10, durability=latch_on))
        self.node.loginfo("Publishing path on /carla/{}/{}".format(
            self._role_name, self._path_topic_name))

        if "launch" in args and "launch-package" in args:

            launch_file = args["launch"]
            launch_package = args["launch-package"]

            if ROS_VERSION == 1:
                cli_args = [launch_package, launch_file]
            elif ROS_VERSION == 2:
                cli_args = [launch_package, launch_file + '.py']
            cli_args.append('role_name:={}'.format(self._role_name))

            # add additional launch parameters
            launch_parameters = []
            for key, value in args.items():
                if not key == "launch" and not key == "launch-package" and not key == "path_topic_name":
                    launch_parameters.append('{}:={}'.format(key, value))
                    cli_args.append('{}:={}'.format(key, value))

            self.node.loginfo(
                "{}: Launching {} from package {} (parameters: {})...".format(
                    self._role_name, launch_file, launch_package,
                    launch_parameters))

            if ROS_VERSION == 1:
                uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
                roslaunch.configure_logging(uuid)
                roslaunch_file = roslaunch.rlutil.resolve_launch_arguments(
                    cli_args)
                roslaunch_args = cli_args[2:]
                launch_files = [(roslaunch_file[0], roslaunch_args)]
                parent = roslaunch.parent.ROSLaunchParent(uuid, launch_files)
                parent.start()

            elif ROS_VERSION == 2:
                cmdline = ['ros2 launch'] + cli_args
                self.controller_launch = ApplicationRunner(
                    self.controller_runner_status_updated,
                    self.controller_runner_log,
                    'RosVehicleControl: launching controller node')
                self.controller_launch.execute(
                    cmdline,
                    env=os.environ,
                )

            self.node.loginfo(
                "{}: Successfully started ros vehicle control".format(
                    self._role_name))
        else:
            self.node.logwarn(
                "{}: Missing value for 'launch' and/or 'launch-package'.".
                format(self._role_name))

    def controller_runner_log(self, log):  # pylint: disable=no-self-use
        """
        Callback for application logs
        """
        self.node.logwarn("[Controller]{}".format(log))

    def controller_runner_status_updated(self, status):
        """
        Executed from application runner whenever the status changed
        """
        self.node.loginfo("Controller status is: {}".format(status))

    def update_target_speed(self, speed):
        super(RosVehicleControl, self).update_target_speed(speed)
        self.node.loginfo("{}: Target speed changed to {}".format(
            self._role_name, speed))
        self._target_speed_publisher.publish(Float64(data=speed))

    def update_waypoints(self, waypoints, start_time=None):
        super(RosVehicleControl, self).update_waypoints(waypoints, start_time)
        self.node.loginfo("{}: Waypoints changed.".format(self._role_name))
        path = Path()
        path.header.stamp = ros_timestamp(sec=self.node.get_time(),
                                          from_sec=True)
        path.header.frame_id = "map"
        for wpt in waypoints:
            print(wpt)
            path.poses.append(
                PoseStamped(pose=trans.carla_transform_to_ros_pose(wpt)))
        self._path_publisher.publish(path)

    def reset(self):
        # set target speed to zero before closing as the controller can take time to shutdown
        if ROS_VERSION == 2:
            self.update_target_speed(0.)
            if self.controller_launch and self.controller_launch.is_running():
                self.controller_launch.shutdown()
        if self._carla_actor and self._carla_actor.is_alive:
            self._carla_actor = None
        if self._target_speed_publisher:
            self.node.destroy_subscription(self._target_speed_publisher)
            self._target_speed_publisher = None
        if self._path_publisher:
            self.node.destroy_subscription(self._path_publisher)
            self._path_publisher = None

    def run_step(self):
        pass
コード例 #2
0
class CarlaControlPlugin(Plugin):
    """
    RQT Plugin to control CARLA
    """
    def __init__(self, context):
        """
        Constructor
        """
        super(CarlaControlPlugin, self).__init__(context)
        self.setObjectName('CARLA Control')

        self._widget = QWidget()

        self._node = CompatibleNode('rqt_carla_control_node', rospy_init=False)

        if ROS_VERSION == 1:
            package_share_dir = rospkg.RosPack().get_path('rqt_carla_control')
            self.callback_group = None
        elif ROS_VERSION == 2:
            package_share_dir = get_package_share_directory(
                'rqt_carla_control')
            self.callback_group = ReentrantCallbackGroup()

        ui_file = os.path.join(package_share_dir, 'resource',
                               'CarlaControl.ui')

        loadUi(ui_file, self._widget)
        self._widget.setObjectName('CarlaControl')
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() +
                                        (' (%d)' % context.serial_number()))

        self.pause_icon = QIcon(
            QPixmap(os.path.join(package_share_dir, 'resource', 'pause.png')))
        self.play_icon = QIcon(
            QPixmap(os.path.join(package_share_dir, 'resource', 'play.png')))
        self._widget.pushButtonStepOnce.setIcon(
            QIcon(
                QPixmap(
                    os.path.join(package_share_dir, 'resource',
                                 'step_once.png'))))

        self.carla_status = None
        self.carla_status_subscriber = self._node.create_subscriber(
            CarlaStatus,
            "/carla/status",
            self.carla_status_changed,
            callback_group=self.callback_group)

        self.carla_control_publisher = self._node.new_publisher(
            CarlaControl, "/carla/control",
            QoSProfile(depth=10, durability=False))

        self._widget.pushButtonPlayPause.setDisabled(True)
        self._widget.pushButtonStepOnce.setDisabled(True)
        self._widget.pushButtonPlayPause.setIcon(self.play_icon)
        self._widget.pushButtonPlayPause.clicked.connect(
            self.toggle_play_pause)
        self._widget.pushButtonStepOnce.clicked.connect(self.step_once)

        context.add_widget(self._widget)

        if ROS_VERSION == 2:
            spin_thread = threading.Thread(target=self._node.spin, daemon=True)
            spin_thread.start()

    def toggle_play_pause(self):
        """
        toggle play/pause
        """
        if self.carla_status.synchronous_mode:
            if self.carla_status.synchronous_mode_running:
                self.carla_control_publisher.publish(
                    CarlaControl(command=CarlaControl.PAUSE))
            else:
                self.carla_control_publisher.publish(
                    CarlaControl(command=CarlaControl.PLAY))

    def step_once(self):
        """
        execute one step
        """
        self.carla_control_publisher.publish(
            CarlaControl(command=CarlaControl.STEP_ONCE))

    def carla_status_changed(self, status):
        """
        callback whenever carla status changes
        """
        self.carla_status = status
        if status.synchronous_mode:
            self._widget.pushButtonPlayPause.setDisabled(False)
            self._widget.pushButtonStepOnce.setDisabled(False)
            if status.synchronous_mode_running:
                self._widget.pushButtonPlayPause.setIcon(self.pause_icon)
            else:
                self._widget.pushButtonPlayPause.setIcon(self.play_icon)
        else:
            self._widget.pushButtonPlayPause.setDisabled(True)
            self._widget.pushButtonStepOnce.setDisabled(True)

    def shutdown_plugin(self):
        """
        shutdown plugin
        """
        self._node.destroy_subscription(self.carla_control_publisher)