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")
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)
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()
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')
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)
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)
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 __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
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))
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()
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)
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()
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)
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)
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)
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)
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
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())
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)
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)
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")
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