Пример #1
0
 def __init__(self, node_name):
     super().__init__(node_name)
     self.client = ActionClient(self, NavigateToPose, "NavGoal")
     self.goal_nbr = 0
     self.goal = None
     self.robot_pose_img = Point()
     self.target = Point()
     self.call = None
     self.map_data = None
     self.width = 0
     self.height = 0
     self.map_origin = Point()
     self.resolution = 0
     self.robot_yaw = 0
     self.robot_pose = Point()
     self.tf_buffer = Buffer()
     self.tf_timer = None
     self.listener = TransformListener(self.tf_buffer, self)
     try:
         self.map_srv = self.create_client(GetMap, "/map_server/map")
         self.get_logger().info("Service client created")
         while not self.map_srv.wait_for_service(timeout_sec=1.0):
             self.get_logger().info(
                 'service not available, waiting again...')
     except Exception as e:
         self.get_logger().info(e)
     self.get_logger().info("robot initialization finished")
Пример #2
0
    def __init__(self, save=True, cycle_time=.5, steps=20):
        self.steps = steps
        self.steps_size = 2 * np.pi / self.steps
        self.change_cycle_time(cycle_time)
        self.is_giskard_active = False

        self.giskard_goals = rospy.Publisher('/whole_body_controller/goal',
                                             WholeBodyCommand,
                                             queue_size=10)
        self.pose_pub = rospy.Publisher('/test', PoseStamped, queue_size=10)
        self.wiggle_counter = 0.
        self.tfBuffer = Buffer()
        self.tf = TransformListener(self.tfBuffer)
        self.save = save

        self.wiggle_action_server = actionlib.SimpleActionServer(
            'wiggle_wiggle_wiggle',
            WiggleAction,
            execute_cb=self.wiggle_cb,
            auto_start=False)
        self.wiggle_action_server.start()
        self.giskard_status = rospy.Subscriber(
            '/controller_action_server/move/status',
            GoalStatusArray,
            self.giskard_status_cb,
            queue_size=10)
        rospy.sleep(1.)
    def __init__(self):
        super().__init__('example_blocking_waits_for_transform')

        self._tf_buffer = Buffer()
        self._tf_listener = TransformListener(self._tf_buffer, self)

        self._output_timer = self.create_timer(1.0, self.on_timer)
Пример #4
0
    def __init__(self, root_link, tip_links):
        # tf
        self.tfBuffer = Buffer(rospy.Duration(1))
        self.tf_listener = TransformListener(self.tfBuffer)

        # giskard goal client
        # self.client = SimpleActionClient('move', ControllerListAction)
        self.client = SimpleActionClient('qp_controller/command',
                                         ControllerListAction)
        self.client.wait_for_server()

        # marker server
        self.server = InteractiveMarkerServer("eef_control")
        self.menu_handler = MenuHandler()

        for tip_link in tip_links:
            int_marker = self.make6DofMarker(
                InteractiveMarkerControl.MOVE_ROTATE_3D, root_link, tip_link)
            self.server.insert(
                int_marker,
                self.process_feedback(self.server, self.client, root_link,
                                      tip_link))
            self.menu_handler.apply(self.server, int_marker.name)

        self.server.applyChanges()
Пример #5
0
class SelfPoseListener(Node):
    def __init__(self):
        super().__init__("self_pose_listener")
        self.tf_buffer = Buffer()
        self._tf_listener = TransformListener(self.tf_buffer, self)
        self.self_pose = PoseStamped()

    def get_current_pose(self):
        try:
            tf = self.tf_buffer.lookup_transform("map", "base_link",
                                                 rclpy.time.Time())
            tf_time = self.tf_buffer.get_latest_common_time("map", "base_link")
            self.self_pose = SelfPoseListener.create_pose(tf_time, "map", tf)
        except LookupException as e:
            self.get_logger().warn(
                "Required transformation not found: `{}`".format(str(e)))
            return None

    @staticmethod
    def create_pose(time, frame_id, tf):
        pose = PoseStamped()

        pose.header.stamp = time.to_msg()
        pose.header.frame_id = frame_id

        pose.pose.position.x = tf.transform.translation.x
        pose.pose.position.y = tf.transform.translation.y
        pose.pose.position.z = tf.transform.translation.z
        pose.pose.orientation.x = tf.transform.rotation.x
        pose.pose.orientation.y = tf.transform.rotation.y
        pose.pose.orientation.z = tf.transform.rotation.z
        pose.pose.orientation.w = tf.transform.rotation.w

        return pose
class BlockingWaitsForTransform(Node):
    """
    Wait for a transform syncronously.

    This class is an example of waiting for transforms.
    This will block the executor if used within a callback.
    Coroutine callbacks should be used instead to avoid this.
    See :doc:`examples_tf2_py/async_waits_for_transform.py` for an example.
    """
    def __init__(self):
        super().__init__('example_blocking_waits_for_transform')

        self._tf_buffer = Buffer()
        self._tf_listener = TransformListener(self._tf_buffer, self)

        self._output_timer = self.create_timer(1.0, self.on_timer)

    def on_timer(self):
        from_frame = 'sonar_2'
        to_frame = 'starboard_wheel'
        self.get_logger().info('Waiting for transform from {} to {}'.format(
            from_frame, to_frame))
        try:
            # Block until the transform is available
            when = rclpy.time.Time()
            self._tf_buffer.lookup_transform(to_frame,
                                             from_frame,
                                             when,
                                             timeout=Duration(seconds=5.0))
        except LookupException:
            self.get_logger().info('transform not ready')
        else:
            self.get_logger().info('Got transform')
Пример #7
0
    def __init__(self):
        super().__init__('executor')

        """Update frequency
        Single targets: {baudrate} / ({uart frame length} * {cmd length} * {number of joint})
        = 115200 / (9 * 4 * 18) ~= 177Hz <- autodetect-baudrate on maestro
        = 200000 / (9 * 4 * 1x8) ~= 300Hz <- fixed-baudrate on maestro
        Multiple targets: {baudrate} / ({uart frame length} * ({header length} + {mini-cmd length} * {number of joints}))
        = 115200 / (9 * (3 + 2 * 18)) ~= 320Hz 
        = 200000 / (9 * (3 + 2 * 18)) ~= 560Hz"""

        self.subscription = self.create_subscription(
            Path,
            'sparse_path',
            self.new_trajectory,
            10
        )
        self.spi_bridge = self.create_publisher(
            UInt8MultiArray, 'stm32_cmd', 10
        )

        self.steps_trajectory = []
        self.init_test_path()

        self._tf_buffer = Buffer()
        self._tf_listener = TransformListener(self._tf_buffer, self)
        self.broadcaster = self.create_publisher(TFMessage, '/tf', 10)
        self.publish_transform = get_transform_publisher(self.broadcaster, self.get_clock())

        self.pub = self.create_publisher(JointState, 'joint_states', 10)
        self.base_link_start_height = tf.translation_matrix((0.0, 0.0, 0.1))

        self.path_proxy = PathProxy(self.steps_trajectory, self.get_logger())
        self.trajectory_generator = TrajectoryGenerator(self.path_proxy)
        self.trajectory_encoder = TrajectoryEncoder()
        self.front_point = Pose()
        self.rear_point = Pose()
        self.init_points()

        self.inv_kin_calc = TripodInverseKinematics(self._tf_buffer, self.get_clock())

        self.prev_time = self.get_clock().now()

        first_step = TrajectoryPoint()
        first_step.timestamp = self.prev_time.nanoseconds / 10**9
        first_step.left_moving = True
        first_step.left_pose = tf.translation_matrix((0, 0, 0))
        first_step.right_pose = tf.translation_matrix((0, 0, 0))
        self.prev_trajectory = self.trajectory_generator.generate_trajectory(
            first_step)

        self.first_time = False
        self.timer_callback()
        self.first_time = False
        self.start_timer = self.create_timer(START_DELAY, self.timer_callback)
        self.timer = None
        self.tf_viz_tim = self.create_timer(0.1, self.tf_viz_callback)
Пример #8
0
    def __init__(self):
        super().__init__('example_frame_dumper')

        self._tf_buffer = Buffer()
        self._tf_listener = TransformListener(self._tf_buffer,
                                              self,
                                              spin_thread=False)

        self._output_timer = self.create_timer(1.0, self.on_timer)
    def __init__(self, options):
        super().__init__("tf2pose")

        self._options = options
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self._pub_pose = self.create_publisher(
            PoseStamped, "/autoware_debug_tools/tf2pose/pose", 1)
        self.timer = self.create_timer((1.0 / self._options.hz),
                                       self._on_timer)
Пример #10
0
    def __init__(self):
        super().__init__('example_waits_for_transform')

        self._tf_buffer = Buffer()
        self._tf_listener = TransformListener(self._tf_buffer, self)

        self._output_timer = self.create_timer(1.0, self.on_timer)

        self._lock = threading.Lock()
        self._tf_future = None
        self._from_frame = None
        self._to_frame = None
        self._when = None
Пример #11
0
    def __init__(self):
        super().__init__('minimal_subscriber')
        self.subscription = self.create_subscription(
            Trajectory, '/planning/scenario_planning/trajectory',
            self.trajectory_callback, 10)

        self.subscription  # prevent unused variable warning

        self.goal_pose = None

        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self.ego_poes = None
Пример #12
0
class WaitsForTransform(Node):
    """
    Wait for a transform using callbacks.

    This class is an example of waiting for transforms.
    It avoids blocking the executor by registering a callback to be called when a transform becomes
    available.
    """
    def __init__(self):
        super().__init__('example_waits_for_transform')

        self._tf_buffer = Buffer()
        self._tf_listener = TransformListener(self._tf_buffer, self)

        self._output_timer = self.create_timer(1.0, self.on_timer)

        self._lock = threading.Lock()
        self._tf_future = None
        self._from_frame = None
        self._to_frame = None
        self._when = None

    def on_tf_ready(self, future):
        with self._lock:
            self._tf_future = None
            if future.result():
                try:
                    self._tf_buffer.lookup_transform(self._to_frame,
                                                     self._from_frame,
                                                     self._when)
                except LookupException:
                    self.get_logger().info('transform no longer available')
                else:
                    self.get_logger().info('Got transform')

    def on_timer(self):
        if self._tf_future:
            self.get_logger().warn('Still waiting for transform')
            return

        with self._lock:
            self._from_frame = 'sonar_2'
            self._to_frame = 'starboard_wheel'
            self._when = rclpy.time.Time()
            self._tf_future = self._tf_buffer.wait_for_transform_async(
                self._to_frame, self._from_frame, self._when)
            self._tf_future.add_done_callback(self.on_tf_ready)
            self.get_logger().info(
                'Waiting for transform from {} to {}'.format(
                    self._from_frame, self._to_frame))
Пример #13
0
    def test_await_transform_full_immediately_available(self):
        # wait for a transform that is already available to test short-cut code
        buffer = Buffer()
        clock = rclpy.clock.Clock()
        rclpy_time = clock.now()
        transform = self.build_transform('foo', 'bar', rclpy_time)

        buffer.set_transform(transform, 'unittest')

        coro = buffer.lookup_transform_full_async('foo', rclpy_time, 'bar', rclpy_time, 'foo')
        with self.assertRaises(StopIteration) as cm:
            coro.send(None)

        self.assertEqual(transform, cm.exception.value)
        coro.close()
Пример #14
0
    def test_can_transform_valid_transform(self):
        buffer = Buffer()
        clock = rclpy.clock.Clock()
        rclpy_time = clock.now()
        transform = self.build_transform('foo', 'bar', rclpy_time)

        self.assertEqual(buffer.set_transform(transform, 'unittest'), None)

        self.assertEqual(buffer.can_transform('foo', 'bar', rclpy_time), True)

        output = buffer.lookup_transform('foo', 'bar', rclpy_time)
        self.assertEqual(transform.child_frame_id, output.child_frame_id)
        self.assertEqual(transform.transform.translation.x, output.transform.translation.x)
        self.assertEqual(transform.transform.translation.y, output.transform.translation.y)
        self.assertEqual(transform.transform.translation.z, output.transform.translation.z)
Пример #15
0
    def test_await_transform_delayed(self):
        # wait for a transform that is not yet available
        buffer = Buffer()
        clock = rclpy.clock.Clock()
        rclpy_time = clock.now()
        transform = self.build_transform('foo', 'bar', rclpy_time)

        coro = buffer.lookup_transform_async('foo', 'bar', rclpy_time)
        coro.send(None)

        buffer.set_transform(transform, 'unittest')
        with self.assertRaises(StopIteration) as cm:
            coro.send(None)

        self.assertEqual(transform, cm.exception.value)
        coro.close()
Пример #16
0
class MinimalSubscriber(Node):
    def __init__(self):
        super().__init__('minimal_subscriber')
        self.subscription = self.create_subscription(
            Trajectory, '/planning/scenario_planning/trajectory',
            self.trajectory_callback, 10)

        self.subscription  # prevent unused variable warning

        self.goal_pose = None

        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self.ego_poes = None
        #self.timer = self.create_timer(0.01, self.on_timer)

    def get_ego(self):
        try:
            now = rclpy.time.Time()
            trans = self.tf_buffer.lookup_transform('map', 'base_link', now)
        except TransformException as ex:
            print("cannot transform")
            return
        return trans.transform.translation

    def trajectory_callback(self, msg: Trajectory):
        goal_pos = msg.points[0].pose.position
        ego_pos = self.get_ego()
        dist = sqrt((goal_pos.x - ego_pos.x)**2 + (goal_pos.y - ego_pos.y)**2)
        print(dist)
Пример #17
0
    def __init__(self):
        super().__init__('tf2_listener')

        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)

        self.timer = self.create_timer(0.500, self.listen)
Пример #18
0
    def __init__(self):
        '''initialize attributes and setup subscriber and publisher'''

        super().__init__('kf_hungarian_node')
        self.declare_parameters(
            namespace='',
            parameters=[
                ('global_frame', "camera_link"),
                ('process_noise_cov', [2., 2., 0.5]),
                ('top_down', False),
                ('death_threshold', 3),
                ('measurement_noise_cov', [1., 1., 1.]),
                ('error_cov_post', [1., 1., 1., 10., 10., 10.]),
                ('vel_filter', [0.1, 2.0]),
                ('height_filter', [-2.0, 2.0]),
                ('cost_filter', 1.0)
            ])
        self.global_frame = self.get_parameter("global_frame")._value
        self.death_threshold = self.get_parameter("death_threshold")._value
        self.measurement_noise_cov = self.get_parameter("measurement_noise_cov")._value
        self.error_cov_post = self.get_parameter("error_cov_post")._value
        self.process_noise_cov = self.get_parameter("process_noise_cov")._value
        self.vel_filter = self.get_parameter("vel_filter")._value
        self.height_filter = self.get_parameter("height_filter")._value
        self.top_down = self.get_parameter("top_down")._value
        self.cost_filter = self.get_parameter("cost_filter")._value

        self.obstacle_list = []
        self.max_id = 0
        self.sec = 0
        self.nanosec = 0

        # subscribe to detector 
        self.detection_sub = self.create_subscription(
            ObstacleArray,
            'detection',
            self.callback,
            10)

        # publisher for tracking result
        self.tracker_obstacle_pub = self.create_publisher(ObstacleArray, 'tracking', 10)
        self.tracker_marker_pub = self.create_publisher(MarkerArray, 'marker', 10)

        # setup tf related
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
def init():
    global tfBuffer, tf_listener, barcode_pose_sub, tf_broadcaster
    rospy.init_node('barcode_tf_publisher')
    tfBuffer = Buffer(rospy.Duration(10))
    tf_listener = TransformListener(tfBuffer)
    tf_broadcaster = tf.TransformBroadcaster()
    rospy.sleep(1)
    barcode_pose_sub = rospy.Subscriber('barcode/pose', Barcode, barcode_sub, queue_size=100)
Пример #20
0
    def test_buffer_non_default_cache(self):
        buffer = Buffer(cache_time=rclpy.duration.Duration(seconds=10.0))
        clock = rclpy.clock.Clock()
        rclpy_time = clock.now()
        transform = self.build_transform('foo', 'bar', rclpy_time)

        self.assertEqual(buffer.set_transform(transform, 'unittest'), None)

        self.assertEqual(buffer.can_transform('foo', 'bar', rclpy_time), True)

        output = buffer.lookup_transform('foo', 'bar', rclpy_time)
        self.assertEqual(transform.child_frame_id, output.child_frame_id)
        self.assertEqual(transform.transform.translation.x,
                         output.transform.translation.x)
        self.assertEqual(transform.transform.translation.y,
                         output.transform.translation.y)
        self.assertEqual(transform.transform.translation.z,
                         output.transform.translation.z)
    def __init__(self):
        super().__init__('example_async_waits_for_transform')

        # Create
        self._tf_buffer = Buffer()
        self._tf_listener = TransformListener(self._tf_buffer, self)

        # Try to get the transform every second.
        # If the transform is unavailable the timer callback will wait for it.
        self._output_timer = self.create_timer(1.0, self.on_timer)
    def setUpClass(cls):
        rclpy.init()

        cls.buffer = Buffer()
        cls.node = rclpy.create_node('test_broadcaster_listener')
        cls.broadcaster = TransformBroadcaster(cls.node)
        cls.static_broadcaster = StaticTransformBroadcaster(cls.node)
        cls.listener = TransformListener(
            buffer=cls.buffer, node=cls.node, spin_thread=False)

        cls.executor = rclpy.executors.SingleThreadedExecutor()
        cls.executor.add_node(cls.node)
Пример #23
0
    def __init__(self):
        super().__init__('dummy_path_publisher')

        self._tf_buffer = Buffer()
        self._tf_listener = TransformListener(self._tf_buffer, self)
        self.transform = get_transform_lookup(self._tf_buffer)

        # self.pub = self.create_publisher(Path, 'sparse_path', 10)
        self.pub = self.create_publisher(Path, 'dense_path', 10)

        self.msg_sent = False
        self.create_timer(1, self.send_msg)
class Tf2PoseNode(Node):
    def __init__(self, options):
        super().__init__("tf2pose")

        self._options = options
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self._pub_pose = self.create_publisher(
            PoseStamped, "/autoware_debug_tools/tf2pose/pose", 1)
        self.timer = self.create_timer((1.0 / self._options.hz),
                                       self._on_timer)

    def _on_timer(self):
        try:
            tf = self.tf_buffer.lookup_transform(self._options.tf_from,
                                                 self._options.tf_to,
                                                 rclpy.time.Time())
            time = self.tf_buffer.get_latest_common_time(
                self._options.tf_from, self._options.tf_to)
            pose = Tf2PoseNode.create_pose(time, self._options.tf_from, tf)
            self._pub_pose.publish(pose)
        except LookupException as e:
            print(e)

    @staticmethod
    def create_pose(time, frame_id, tf):
        pose = PoseStamped()

        pose.header.stamp = time.to_msg()
        pose.header.frame_id = frame_id

        pose.pose.position.x = tf.transform.translation.x
        pose.pose.position.y = tf.transform.translation.y
        pose.pose.position.z = tf.transform.translation.z
        pose.pose.orientation.x = tf.transform.rotation.x
        pose.pose.orientation.y = tf.transform.rotation.y
        pose.pose.orientation.z = tf.transform.rotation.z
        pose.pose.orientation.w = tf.transform.rotation.w

        return pose
Пример #25
0
class FrameDumper(Node):
    """Print all frames to stdout at a regular interval."""
    def __init__(self):
        super().__init__('example_frame_dumper')

        self._tf_buffer = Buffer()
        self._tf_listener = TransformListener(self._tf_buffer,
                                              self,
                                              spin_thread=False)

        self._output_timer = self.create_timer(1.0, self.on_timer)

    def on_timer(self):
        print(self._tf_buffer.all_frames_as_yaml())
Пример #26
0
 def __init__(self, node_name: str):
     super().__init__(node_name)
     self.get_logger().info("initializing AutoMapper")
     self.call = None
     self.create_timer(1, self.get_map)
     self.map_data = None
     self.width = 0
     self.height = 0
     self.map_origin = Point()
     self.resolution = 0
     self.robot_yaw = 0
     self.robot_pose = Point()
     self.tf_buffer = Buffer()
     self.tf_timer = None
     self.listener = TransformListener(self.tf_buffer, self)
     try:
         self.map_srv = self.create_client(GetMap, "/map_server/map")
         self.get_logger().info("Service client created")
         while not self.map_srv.wait_for_service(timeout_sec=1.0):
             self.get_logger().info(
                 'service not available, waiting again...')
     except Exception as e:
         self.get_logger().info(e)
Пример #27
0
    def __init__(self):
        super().__init__('sparser')
        self.max_stride = 0.02
        self.max_turn = 5 * pi / 180

        self._tf_buffer = Buffer()
        self._tf_listener = TransformListener(self._tf_buffer, self)
        self.transform = get_transform_lookup(self._tf_buffer)
        self.tf_broadcaster = self.create_publisher(TFMessage, '/tf', 10)
        self.publish_transform = get_transform_publisher(
            self.tf_broadcaster, self.get_clock())

        self.pub = self.create_publisher(Path, 'sparse_path', 10)
        self.curr_path = []
        self.publish_path(self.curr_path)

        self.subscription = self.create_subscription(Path, 'dense_path',
                                                     self.new_trajectory, 10)
Пример #28
0
    def __init__(self):
        super().__init__(node_name="cetautomatix")
        # Detect simulation mode
        self.simulation = True if machine() != "aarch64" else False
        self.i = 0
        self.stupid_actions = ["STUPID_1", "STUPID_2", "STUPID_3"]
        self._triggered = False
        self._position = None
        self._orientation = None
        self._start_time = None
        self._current_action = None
        self.robot = self.get_namespace().strip("/")
        # parameters interfaces
        self.side = self.declare_parameter("side", "blue")
        self.declare_parameter("length")
        self.declare_parameter("width")
        self.declare_parameter("strategy_mode")
        self.length_param = self.get_parameter("length")
        self.width_param = self.get_parameter("width")
        self.strategy_mode_param = self.get_parameter("strategy_mode")
        # Bind actuators
        self.actuators = import_module(f"actuators.{self.robot}").actuators
        # Do selftest
        self.selftest = Selftest(self)
        # Prechill engines
        self.actuators.setFansEnabled(True)
        # Stop ros service
        self._stop_ros_service = self.create_service(Trigger, "stop",
                                                     self._stop_robot_callback)
        # strategix client interfaces
        self._get_available_request = GetAvailableActions.Request()
        self._get_available_request.sender = self.robot
        self._get_available_client = self.create_client(
            GetAvailableActions, "/strategix/available")
        self._change_action_status_request = ChangeActionStatus.Request()
        self._change_action_status_request.sender = self.robot
        self._change_action_status_client = self.create_client(
            ChangeActionStatus, "/strategix/action")
        # Phararon delploy client interfaces
        self._get_trigger_deploy_pharaon_request = Trigger.Request()
        self._get_trigger_deploy_pharaon_client = self.create_client(
            Trigger, "/pharaon/deploy")
        # Slider driver
        self._set_slider_position_request = Slider.Request()
        self._set_slider_position_client = self.create_client(
            Slider, "slider_position")
        # Odometry subscriber
        self._tf_buffer = Buffer()
        self._tf_listener = TransformListener(self._tf_buffer, self)
        self._odom_pose_stamped = tf2_geometry_msgs.PoseStamped()
        self._odom_callback(self._odom_pose_stamped)
        self._odom_sub = self.create_subscription(Odometry, "odom",
                                                  self._odom_callback, 1)
        # Py-Trees blackboard to send NavigateToPose actions
        self.blackboard = py_trees.blackboard.Client(name="NavigateToPose")
        self.blackboard.register_key(key="goal",
                                     access=py_trees.common.Access.WRITE)
        # LCD driver direct access
        self._lcd_driver_pub = self.create_publisher(Lcd, "/obelix/lcd", 1)
        # Wait for strategix as this can block the behavior Tree
        while not self._get_available_client.wait_for_service(timeout_sec=5):
            self.get_logger().warn(
                "Failed to contact strategix services ! Has it been started ?")
        # Enable stepper drivers
        if not self.simulation:
            self._get_enable_drivers_request = SetBool.Request()
            self._get_enable_drivers_request.data = True
            self._get_enable_drivers_client = self.create_client(
                SetBool, "enable_drivers")
            self._synchronous_call(self._get_enable_drivers_client,
                                   self._get_enable_drivers_request)
        # Robot trigger service
        self._trigger_start_robot_server = self.create_service(
            Trigger, "start", self._start_robot_callback)
        if self.robot == "obelix":
            self._trigger_start_asterix_request = Trigger.Request()
            self._trigger_start_asterix_client = self.create_client(
                Trigger, "/asterix/start")

        # Reached initialized state
        self.get_logger().info("Cetautomatix ROS node has been started")
Пример #29
0
class Robot(Node):
    def __init__(self):
        super().__init__(node_name="cetautomatix")
        # Detect simulation mode
        self.simulation = True if machine() != "aarch64" else False
        self.i = 0
        self.stupid_actions = ["STUPID_1", "STUPID_2", "STUPID_3"]
        self._triggered = False
        self._position = None
        self._orientation = None
        self._start_time = None
        self._current_action = None
        self.robot = self.get_namespace().strip("/")
        # parameters interfaces
        self.side = self.declare_parameter("side", "blue")
        self.declare_parameter("length")
        self.declare_parameter("width")
        self.declare_parameter("strategy_mode")
        self.length_param = self.get_parameter("length")
        self.width_param = self.get_parameter("width")
        self.strategy_mode_param = self.get_parameter("strategy_mode")
        # Bind actuators
        self.actuators = import_module(f"actuators.{self.robot}").actuators
        # Do selftest
        self.selftest = Selftest(self)
        # Prechill engines
        self.actuators.setFansEnabled(True)
        # Stop ros service
        self._stop_ros_service = self.create_service(Trigger, "stop",
                                                     self._stop_robot_callback)
        # strategix client interfaces
        self._get_available_request = GetAvailableActions.Request()
        self._get_available_request.sender = self.robot
        self._get_available_client = self.create_client(
            GetAvailableActions, "/strategix/available")
        self._change_action_status_request = ChangeActionStatus.Request()
        self._change_action_status_request.sender = self.robot
        self._change_action_status_client = self.create_client(
            ChangeActionStatus, "/strategix/action")
        # Phararon delploy client interfaces
        self._get_trigger_deploy_pharaon_request = Trigger.Request()
        self._get_trigger_deploy_pharaon_client = self.create_client(
            Trigger, "/pharaon/deploy")
        # Slider driver
        self._set_slider_position_request = Slider.Request()
        self._set_slider_position_client = self.create_client(
            Slider, "slider_position")
        # Odometry subscriber
        self._tf_buffer = Buffer()
        self._tf_listener = TransformListener(self._tf_buffer, self)
        self._odom_pose_stamped = tf2_geometry_msgs.PoseStamped()
        self._odom_callback(self._odom_pose_stamped)
        self._odom_sub = self.create_subscription(Odometry, "odom",
                                                  self._odom_callback, 1)
        # Py-Trees blackboard to send NavigateToPose actions
        self.blackboard = py_trees.blackboard.Client(name="NavigateToPose")
        self.blackboard.register_key(key="goal",
                                     access=py_trees.common.Access.WRITE)
        # LCD driver direct access
        self._lcd_driver_pub = self.create_publisher(Lcd, "/obelix/lcd", 1)
        # Wait for strategix as this can block the behavior Tree
        while not self._get_available_client.wait_for_service(timeout_sec=5):
            self.get_logger().warn(
                "Failed to contact strategix services ! Has it been started ?")
        # Enable stepper drivers
        if not self.simulation:
            self._get_enable_drivers_request = SetBool.Request()
            self._get_enable_drivers_request.data = True
            self._get_enable_drivers_client = self.create_client(
                SetBool, "enable_drivers")
            self._synchronous_call(self._get_enable_drivers_client,
                                   self._get_enable_drivers_request)
        # Robot trigger service
        self._trigger_start_robot_server = self.create_service(
            Trigger, "start", self._start_robot_callback)
        if self.robot == "obelix":
            self._trigger_start_asterix_request = Trigger.Request()
            self._trigger_start_asterix_client = self.create_client(
                Trigger, "/asterix/start")

        # Reached initialized state
        self.get_logger().info("Cetautomatix ROS node has been started")

    def _synchronous_call(self, client, request):
        """Call service synchronously."""
        future = client.call_async(request)
        rclpy.spin_until_future_complete(self, future)
        try:
            response = future.result()
        except BaseException:
            response = None
        return response

    def set_slider_position(self, position: int):
        """Set slider position with position in range 0 to 255."""
        self._set_slider_position_request.position = position
        return self._synchronous_call(self._set_slider_position_client,
                                      self._set_slider_position_request)

    def fetch_available_actions(self):
        """Fetch available actions for BT."""
        response = self._synchronous_call(self._get_available_client,
                                          self._get_available_request)
        return response.available if response is not None else []

    def preempt_action(self, action):
        """Preempt an action for the BT."""
        if action is None:
            return False
        self._change_action_status_request.action = str(action)
        self._change_action_status_request.request = "PREEMPT"
        response = self._synchronous_call(self._change_action_status_client,
                                          self._change_action_status_request)
        if response is None:
            return False
        self._current_action = action if response.success else None
        if self._current_action is not None:
            self.get_goal_pose()
        return response.success

    def drop_current_action(self):
        """Drop an action for the BT."""
        if self._current_action is None:
            return False
        self._change_action_status_request.action = self._current_action
        self._change_action_status_request.request = "DROP"
        response = self._synchronous_call(self._change_action_status_client,
                                          self._change_action_status_request)
        if response is None:
            return False
        self._current_action = None
        return response.success

    def confirm_current_action(self):
        """Confirm an action for the BT."""
        if self._current_action is None:
            return False
        self._change_action_status_request.action = self._current_action
        self._change_action_status_request.request = "CONFIRM"
        response = self._synchronous_call(self._change_action_status_client,
                                          self._change_action_status_request)
        if response is None:
            return False
        self._current_action = None
        return response.success

    def start_actuator_action(self):
        self.get_logger().info(f"START ACTUATOR {self._current_action}")
        if "PHARE" in self._current_action:
            response = self._synchronous_call(
                self._get_trigger_deploy_pharaon_client,
                self._get_trigger_deploy_pharaon_request,
            )
            return response.success
        elif "GOB" in self._current_action:
            self.actuators.PUMPS[
                self.current_pump]["STATUS"] = self._current_action
            servo = self.actuators.DYNAMIXELS[self.current_pump]
            self.actuators.arbotix.setPosition(self.current_pump,
                                               servo.get("up"))
            return True
        elif "ECUEIL" in self._current_action:
            pump_list = []
            i = 0
            for pump_id, pump_dict in self.actuators.PUMPS.items():
                if pump_dict.get("type") == NO:
                    pump_list.append(pump_id)
                    self.actuators.PUMPS[pump_id]["STATUS"] = actions[
                        self._current_action].get("GOBS")[i]
                    i += 1
            self.actuators.setPumpsEnabled(True, pump_list)
            # TODO: Fermer Herse
            self.set_slider_position(255)
            return True
        elif "CHENAL" in self._current_action:
            color = "GREEN" if "VERT" in self._current_action else "RED"
            pump_list = []
            for pump_id, pump_dict in self.actuators.PUMPS.items():
                if (pump_dict.get("type") == self.drop
                        and pump_dict.get("STATUS") == color):
                    pump_list.append(pump_id)
            if self.drop == NC:
                for pump in pump_list:
                    servo = self.actuators.DYNAMIXELS[pump]
                    self.actuators.arbotix.setPosition(pump, servo.get("down"))
            else:
                self.set_slider_position(100)
                # TODO: Ouvrir herse
            self.actuators.setPumpsEnabled(False, pump_list)
            for pump in pump_list:
                del self.actuators.PUMPS[pump]["STATUS"]
            return True
        else:
            return True

    def trigger_pavillons(self):
        self.get_logger().info("Triggered pavillons")
        self.actuators.raiseTheFlag()
        self._change_action_status_request.action = "PAVILLON"
        self._change_action_status_request.request = "CONFIRM"
        response = self._synchronous_call(self._change_action_status_client,
                                          self._change_action_status_request)
        if response is None:
            return False
        return response.success

    def _start_robot_callback(self, req, resp):
        """Start robot."""
        self._triggered = True
        self.get_logger().info("Triggered robot starter")
        if self.robot == "obelix":
            # TODO : Fix sync call
            # Thread(target=self._synchronous_call, args=[self._trigger_start_asterix_client, self._trigger_start_asterix_request]).start()
            self.get_logger().info("Obelix triggered Asterix")
        self._start_time = self.get_clock().now().nanoseconds * 1e-9
        lcd_msg = Lcd()
        lcd_msg.line = 0
        lcd_msg.text = f"{self.robot.capitalize()} running".ljust(16)
        self._lcd_driver_pub.publish(lcd_msg)
        lcd_msg.line = 1
        lcd_msg.text = "Score: 0".ljust(16)
        self._lcd_driver_pub.publish(lcd_msg)
        if hasattr(resp, "success"):
            resp.success = True
        return resp

    def _stop_robot_callback(self, req, resp):
        """Stop robot / ROS."""
        self.stop_ros(shutdown=False)
        resp.sucess = True
        return resp

    def triggered(self):
        """Triggered var."""
        return self._triggered

    def _odom_callback(self, msg):
        try:
            # Get latest transform
            tf = self._tf_buffer.lookup_transform(
                "map", "base_link", Time(), timeout=Duration(seconds=1.0))
            self._odom_pose_stamped.header = msg.header
            self._odom_pose_stamped.pose = msg.pose.pose
            tf_pose = tf2_geometry_msgs.do_transform_pose(
                self._odom_pose_stamped, tf)
        except LookupException:
            self.get_logger().warn(
                "Failed to lookup_transform from map to odom")
            return
        except ConnectivityException:
            self.get_logger().warn(
                "ConnectivityException, 'map' and 'base_link' are not part of the same tree"
            )
            return
        self._position = (tf_pose.pose.position.x, tf_pose.pose.position.y)
        self._orientation = self.quaternion_to_euler(
            tf_pose.pose.orientation.x,
            tf_pose.pose.orientation.y,
            tf_pose.pose.orientation.z,
            tf_pose.pose.orientation.w,
        )

    def euler_to_quaternion(self, yaw, pitch=0, roll=0):
        """Conversion between euler angles and quaternions."""
        qx = np.sin(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) - np.cos(
            roll / 2) * np.sin(pitch / 2) * np.sin(yaw / 2)
        qy = np.cos(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2) + np.sin(
            roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2)
        qz = np.cos(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2) - np.sin(
            roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2)
        qw = np.cos(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) + np.sin(
            roll / 2) * np.sin(pitch / 2) * np.sin(yaw / 2)
        return (qx, qy, qz, qw)

    def quaternion_to_euler(self, x, y, z, w):
        """Conversion between quaternions and euler angles."""
        t0 = +2.0 * (w * x + y * z)
        t1 = +1.0 - 2.0 * (x * x + y * y)
        X = math.atan2(t0, t1)
        t2 = +2.0 * (w * y - z * x)
        t2 = +1.0 if t2 > +1.0 else t2
        t2 = -1.0 if t2 < -1.0 else t2
        Y = math.asin(t2)
        t3 = +2.0 * (w * z + x * y)
        t4 = +1.0 - 2.0 * (y * y + z * z)
        Z = math.atan2(t3, t4)
        return (X, Y, Z)

    def get_position(self, element, initial_orientation, diff_orientation,
                     offset):
        element = elements[element]
        element_x, element_y = element.get("X"), element.get("Y")
        offset_x, offset_y = offset[0], offset[1]

        alpha = initial_orientation[0] - np.pi / 2
        v = (
            offset_x * np.cos(alpha) - offset_y * np.sin(alpha),
            offset_x * np.sin(alpha) + offset_y * np.cos(alpha),
        )
        v = (
            v[0] * np.cos(diff_orientation) - v[1] * np.sin(diff_orientation),
            v[0] * np.sin(diff_orientation) + v[1] * np.cos(diff_orientation),
        )
        return (element_x - v[0], element_y - v[1])

    def get_orientation(self, element, robot_pos, initial_orientation):
        element = elements[element]
        element_x, element_y = element.get("X"), element.get("Y")
        robot_pos_x, robot_pos_y = robot_pos[0], robot_pos[1]

        u = (np.cos(initial_orientation[0]), np.sin(initial_orientation[0]))
        u = u / (np.sqrt(u[0]**2 + u[1]**2))
        if element.get("Rot") is None:
            v = (element_x - robot_pos_x, element_y - robot_pos_y)
            v = v / (np.sqrt(v[0]**2 + v[1]**2))
            theta = np.arccos(
                np.dot(u, v) /
                (np.sqrt(u[0]**2 + u[1]**2) * np.sqrt(v[0]**2 + v[1]**2)))
            if robot_pos_y <= element_y:
                theta = theta
            else:
                theta = -theta
        else:
            angle = element.get("Rot")
            theta = np.deg2rad(angle) - initial_orientation[0]
        phi = initial_orientation[0] + theta
        return (theta, phi)

    def compute_best_action(self, action_list):
        """Calculate best action to choose from its distance to the robot."""
        return self.stupid_actions[self.i % len(self.stupid_actions)]
        check = self.check_to_empty()
        self.get_logger().info(f"{check}")
        if check is not None:
            action_list = check
        if not action_list:
            return None
        coefficient_list = []
        for action in action_list:
            coefficient = 0
            element = elements[action]
            distance = np.sqrt((element["X"] - self._position[0])**2 +
                               (element["Y"] - self._position[1])**2)
            coefficient += 100 * (1 - distance / 3.6)
            coefficient += get_time_coeff(
                self.get_clock().now().nanoseconds * 1e-9 - self._start_time,
                action,
                self.strategy_mode_param.value,
            )
            coefficient_list.append(coefficient)
        best_action = action_list[coefficient_list.index(
            max(coefficient_list))]
        return best_action

    def check_to_empty(self):
        empty_behind, empty_front = True, True
        for pump_id, pump_dict in self.actuators.PUMPS.items():
            if pump_dict.get("STATUS") is None:
                if pump_dict.get("type") == NO:
                    empty_behind = False
                else:
                    empty_front = False
        self.get_logger().info(f"behind: {empty_behind}, front: {empty_front}")
        if empty_behind or empty_front:
            if self.side.value == "blue":
                return ["CHENAL_BLEU_VERT_1", "CHENAL_BLEU_ROUGE_1"]
            else:
                return ["CHENAL_JAUNE_VERT_1", "CHENAL_JAUNE_ROUGE_1"]
        return None

    def get_goal_pose(self):
        """Get goal pose for action."""
        msg = NavigateToPose_Goal()
        msg.pose.header.frame_id = "map"
        # if self._current_action is not None:
        #     offset = (0, 0)
        #     if 'GOB' in self._current_action:
        #         for pump_id, pump_dict in self.actuators.PUMPS.items():
        #             if pump_dict.get('type') == NC and not pump_dict.get('STATUS'):
        #                 self.current_pump = pump_id
        #                 offset = pump_dict.get("pos")
        #                 self.actuators.setPumpsEnabled(True, [pump_id])
        #                 break
        #     elif 'ECUEIL' in self._current_action:
        #         for pump_id, pump_dict in self.actuators.PUMPS.items():
        #             if pump_dict.get('type') == NO and pump_dict.get('STATUS') is not None:
        #                 offset = pump_dict.get("pos")
        #                 break
        #         self.set_slider_position(0)
        #     elif 'CHENAL' in self._current_action:
        #         arriere = False
        #         for pump_id, pump_dict in self.actuators.PUMPS.items():
        #             if pump_dict.get('type') == NO and pump_dict.get('STATUS') is not None:
        #                 arriere = True
        #         if arriere is True:
        #             offset = (0, -0.1)
        #             elements[self._current_action]["Rot"] = -90
        #             self.drop = NO
        #         else:
        #             offset = (0, 0.1)
        #             elements[self._current_action]["Rot"] = 90
        #             self.drop = NC
        if self._position is not None:
            offset = (0, 0)
            (theta, phi) = self.get_orientation(self._current_action,
                                                self._position,
                                                self._orientation)
            position = self.get_position(self._current_action,
                                         self._orientation, theta, offset)
            msg.pose.pose.position.x = position[0]
            msg.pose.pose.position.y = position[1]
            msg.pose.pose.position.z = 0.0
            q = self.euler_to_quaternion(phi)
            msg.pose.pose.orientation.x = q[0]
            msg.pose.pose.orientation.y = q[1]
            msg.pose.pose.orientation.z = q[2]
            msg.pose.pose.orientation.w = q[3]
        self.blackboard.goal = msg
        self.i += 1

    def stop_ros(self, shutdown=True):
        """Stop ros launch processes."""
        # Publish "Robot is done"
        lcd_msg = Lcd()
        lcd_msg.line = 0
        lcd_msg.text = f"{self.robot.capitalize()} is done!".ljust(16)
        self._lcd_driver_pub.publish(lcd_msg)
        if not self.simulation:
            # Disable stepper drivers
            self._get_enable_drivers_request.data = False
            self._synchronous_call(self._get_enable_drivers_client,
                                   self._get_enable_drivers_request)
            # Stop fans and relax servos
            self.actuators.disableDynamixels()
            self.actuators.setFansEnabled(False)
            # Shutdown ROS
            for p in psutil.process_iter(["pid", "name", "cmdline"]):
                if "ros2" in p.name() and "launch" in p.cmdline():
                    self.get_logger().warn(
                        f"Sent SIGINT to ros2 launch {p.pid}")
                    p.send_signal(SIGINT)
            # shutdown linux
            if shutdown:
                call(["shutdown", "-h", "now"])

    def destroy_node(self):
        """Handle SIGINT/global destructor."""
        self.actuators.disableDynamixels()
        self.actuators.setFansEnabled(False)
        super().destroy_node()
    def __init__(self):

        super().__init__("trajectory_visualizer")

        self.fig = plt.figure()

        self.max_vel = 0.0
        self.min_vel = 0.0
        self.min_accel = 0.0
        self.max_accel = 0.0
        self.min_jerk = 0.0
        self.max_jerk = 0.0

        # update flag
        self.update_ex_vel_lim = False
        self.update_lat_acc_fil = False
        self.update_traj_raw = False
        self.update_traj_resample = False
        self.update_traj_final = False
        self.update_lanechange_path = False
        self.update_behavior_path = False
        self.update_traj_ob_avoid = False
        self.update_traj_ob_stop = False

        self.tf_buffer = Buffer(node=self)
        self.tf_listener = TransformListener(self.tf_buffer,
                                             self,
                                             spin_thread=True)

        self.self_pose = Pose()
        self.self_pose_received = False
        self.localization_vx = 0.0
        self.vehicle_vx = 0.0

        self.trajectory_external_velocity_limited = Trajectory()
        self.trajectory_lateral_acc_filtered = Trajectory()
        self.trajectory_raw = Trajectory()
        self.trajectory_time_resampled = Trajectory()
        self.trajectory_final = Trajectory()

        self.lane_change_path = PathWithLaneId()
        self.behavior_path = Path()
        self.obstacle_avoid_traj = Trajectory()
        self.obstacle_stop_traj = Trajectory()

        self.plotted = [False] * 9
        self.sub_localization_twist = self.create_subscription(
            Odometry, "/localization/kinematic_state",
            self.CallbackLocalizationTwist, 1)
        self.sub_vehicle_twist = self.create_subscription(
            VelocityReport, "/vehicle/status/velocity_status",
            self.CallbackVehicleTwist, 1)

        # BUFFER_SIZE = 65536*100
        optimizer_debug = "/planning/scenario_planning/motion_velocity_smoother/debug/"
        self.sub1 = message_filters.Subscriber(
            self, Trajectory,
            optimizer_debug + "trajectory_external_velocity_limited")
        self.sub2 = message_filters.Subscriber(
            self, Trajectory,
            optimizer_debug + "trajectory_lateral_acc_filtered")
        self.sub3 = message_filters.Subscriber(
            self, Trajectory, optimizer_debug + "trajectory_raw")
        self.sub4 = message_filters.Subscriber(
            self, Trajectory, optimizer_debug + "trajectory_time_resampled")
        self.sub5 = message_filters.Subscriber(
            self, Trajectory, "/planning/scenario_planning/trajectory")

        lane_driving = "/planning/scenario_planning/lane_driving"
        self.sub6 = message_filters.Subscriber(
            self, PathWithLaneId,
            lane_driving + "/behavior_planning/path_with_lane_id")
        self.sub7 = message_filters.Subscriber(
            self, Path, lane_driving + "/behavior_planning/path")
        self.sub8 = message_filters.Subscriber(
            self,
            Trajectory,
            lane_driving +
            "/motion_planning/obstacle_avoidance_planner/trajectory",
        )
        self.sub9 = message_filters.Subscriber(
            self, Trajectory, "/planning/scenario_planning/trajectory")

        self.ts1 = message_filters.ApproximateTimeSynchronizer(
            [self.sub1, self.sub2, self.sub3, self.sub4, self.sub5], 30, 0.5)
        self.ts1.registerCallback(self.CallbackMotionVelOptTraj)

        self.ts2 = message_filters.ApproximateTimeSynchronizer(
            [self.sub6, self.sub7, self.sub8, self.sub9], 30, 1, 0)
        self.ts2.registerCallback(self.CallBackLaneDrivingTraj)

        # main process
        if PLOT_TYPE == "VEL_ACC_JERK":
            self.ani = animation.FuncAnimation(self.fig,
                                               self.plotTrajectory,
                                               interval=100,
                                               blit=True)
            self.setPlotTrajectory()
        else:
            self.ani = animation.FuncAnimation(self.fig,
                                               self.plotTrajectoryVelocity,
                                               interval=100,
                                               blit=True)
            self.setPlotTrajectoryVelocity()

        plt.show()

        return