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, 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()
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):
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, 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():
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 __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):
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, 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_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)
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
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_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_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()
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__(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 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, 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__("velocity_checker")
self.autoware_engage = None
self.external_v_lim = np.nan
self.localization_twist = Twist()
self.localization_twist.linear.x = np.nan
self.vehicle_twist = Twist()
self.vehicle_twist.linear.x = np.nan
self.self_pose = Pose()
self.data_arr = [np.nan] * DATA_NUM
self.count = 0
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
# planning path and trajectories
profile = rclpy.qos.QoSProfile(
depth=1, durability=rclpy.qos.QoSDurabilityPolicy.TRANSIENT_LOCAL)
lane_drv = "/planning/scenario_planning/lane_driving"
scenario = "/planning/scenario_planning"
self.sub0 = self.create_subscription(
PathWithLaneId,
lane_drv + "/behavior_planning/path_with_lane_id",
self.CallBackBehaviorPathWLid,
1,
)
self.sub1 = self.create_subscription(
Path, lane_drv + "/behavior_planning/path",
self.CallBackBehaviorPath, 1)
self.sub2 = self.create_subscription(
Trajectory,
lane_drv +
"/motion_planning/obstacle_avoidance_planner/trajectory",
self.CallBackAvoidTrajectory,
1,
)
self.sub3 = self.create_subscription(Trajectory,
lane_drv + "/trajectory",
self.CallBackLaneDriveTrajectory,
1)
self.sub4 = self.create_subscription(
Trajectory,
scenario +
"/motion_velocity_optimizer/debug/trajectory_lateral_acc_filtered",
self.CallBackLataccTrajectory,
1,
)
self.sub5 = self.create_subscription(Trajectory,
scenario + "/trajectory",
self.CallBackScenarioTrajectory,
1)
# control commands
self.sub6 = self.create_subscription(
ControlCommandStamped,
"/control/trajectory_follower/control_cmd",
self.CallBackControlCmd,
1,
)
self.sub7 = self.create_subscription(VehicleCommand,
"/control/vehicle_cmd",
self.CallBackVehicleCmd, 1)
# others related to velocity
self.sub8 = self.create_subscription(Engage, "/autoware/engage",
self.CallBackAwEngage, profile)
self.sub9 = self.create_subscription(
VelocityLimit,
"/planning/scenario_planning/current_max_velocity",
self.CallBackExternalVelLim,
profile,
)
# self twist
self.sub10 = self.create_subscription(TwistStamped,
"/localization/twist",
self.CallBackLocalizationTwist,
1)
self.sub11 = self.create_subscription(
Odometry, "/vehicle/status/velocity_status",
self.CallBackVehicleTwist, 1)
# publish data
self.pub_v_arr = self.create_publisher(Float32MultiArrayStamped,
"closest_speeds", 1)
time.sleep(1.0) # wait for ready to publish/subscribe
# for publish traffic signal image
self.create_timer(0.1, self.timerCallback)
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 __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
def __init__(self, robot_name, mode="real_robot"):
super().__init__('robot_track_publisher_' + robot_name)
self.DEBUG_ = True
self.para_group = ReentrantCallbackGroup()
self.local_frontiers_ = [
] #list of frontier, each is list of (double, double) in the local map frame
self.local_frontiers_msg_ = [
] #list of multi_robot_interfaces.msg.Frontier
self.global_frontiers_ = []
self.robot_name_ = robot_name
self.current_state_ = 0
self.previous_state_ = -1
#current robot_peers
self.robot_peers_ = []
self.mode_ = mode
#all the robot_peers ever discovered in history, no matter current network status
self.persistent_robot_peers_ = []
#the ever growing merged global_map of current robot, will update with new peer_map and new local_map
self.persistent_global_map_ = None
#offset from robot_peers to the self.local_fixed_frame_, {'tb0': (x_offset, y_offset) , 'tb1':(x_offset, y_offset), ...}
self.persistent_offset_from_peer_to_local_ = dict()
#current local_map
self.local_map_ = None
self.inflated_local_map_ = None
self.current_pos_ = (-1, -1)
self.current_pose_local_frame_ = PoseStamped()
self.current_target_pos_ = Pose()
self.next_target_pos_ = Pose()
self.world_frame_ = ''
self.local_fixed_frame_ = ''
if self.robot_name_ == '':
self.world_frame_ = 'map'
self.local_fixed_frame_ = 'base_link'
else:
self.world_frame_ = self.robot_name_ + '/map'
self.local_fixed_frame_ = self.robot_name_ + '/base_link'
self._tf_buffer = Buffer()
self._tf_listener = TransformListener(self._tf_buffer, self)
timer_period = 2 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
self.publisher_ = self.create_publisher(RobotTrack, 'robot_track', 10)
self.robot_pose_sub_ = self.create_subscription(
Point, self.robot_name_ + '/robot_pose', self.robotPoseCallback,
10)
self.robot_pose_sub_ # prevent unused variable warning
self.init_offset_dict_ = dict()
self.init_offset_to_current_robot_dict_ = dict()
self.declare_parameters(namespace='',
parameters=[('robot_name', None),
('peer_list', None),
('simulation_mode', None),
('tb0_init_offset', None),
('tb1_init_offset', None),
('tb2_init_offset', None),
('tb3_init_offset', None),
('tb4_init_offset', None),
('tb5_init_offset', None),
('tb6_init_offset', None),
('tb7_init_offset', None),
('tb8_init_offset', None),
('tb9_init_offset', None),
('pid', None)])
# qos_profile=qos_profile_sensor_data)
# rclpy.spin_once(self)
peer_list_param_name = 'peer_list'
self.persistent_robot_peers_ = self.get_parameter(
peer_list_param_name).value
self.e_util = ExploreUtil()
self.getPeerRobotInitPose()
def __init__(self, robot_name, total_robot_num):
super().__init__('multi_explore_swarm_simulation_' + robot_name)
self.DEBUG_ = True
self.total_robot_num_ = int(total_robot_num)
self.para_group = ReentrantCallbackGroup()
self.local_frontiers_ = [
] #list of frontier, each is list of (double, double) in the local map frame
self.local_frontiers_msg_ = [
] #list of multi_robot_interfaces.msg.Frontier
self.global_frontiers_ = []
self.robot_name_ = robot_name
self.current_state_ = 0
self.previous_state_ = -1
#current robot_peers
self.robot_peers_ = []
#all the robot_peers ever discovered in history, no matter current network status
self.persistent_robot_peers_ = []
#the ever growing merged global_map of current robot, will update with new peer_map and new local_map
self.persistent_global_map_ = None
#offset from robot_peers to the self.local_fixed_frame_, {'tb0': (x_offset, y_offset) , 'tb1':(x_offset, y_offset), ...}
self.persistent_offset_from_peer_to_local_ = dict()
#current local_map
self.local_map_ = None
self.inflated_local_map_ = None
self.current_pos_ = (-1, -1)
self.current_pose_local_frame_ = PoseStamped()
self.current_target_pos_ = Pose()
self.next_target_pos_ = Pose()
#multi robot settings
self.peer_map_ = dict()
self.peer_local_frontiers_ = dict()
self.peer_data_updated_ = dict()
self.merge_map_frontier_timeout_ = 5
self.merged_map_ = None
self.merged_frontiers_ = []
self.world_frame_ = ''
self.local_fixed_frame_ = ''
if self.robot_name_ == '':
self.world_frame_ = 'map'
self.local_fixed_frame_ = 'base_link'
else:
self.world_frame_ = self.robot_name_ + '/map'
self.local_fixed_frame_ = self.robot_name_ + '/base_link'
self._tf_buffer = Buffer()
self._tf_listener = TransformListener(self._tf_buffer, self)
# self.local_map_srv = self.create_service(GetLocalMap, self.robot_name_ + '/get_local_map', self.getLocalMapCallback)
self.local_map_and_frontier_srv = self.create_service(
GetLocalMapAndFrontier,
self.robot_name_ + '/get_local_map_and_frontier',
self.getLocalMapAndFrontierCallback)
self.local_map_and_frontier_srv = self.create_service(
GetLocalMapAndFrontierCompress,
self.robot_name_ + '/get_local_map_and_frontier_compress',
self.getLocalMapAndFrontierCompressCallback)
# self.get_map_value_srv = self.create_service(GetPeerMapValueOnCoords, self.robot_name_ + '/get_map_value_on_coords', self.getMapValueOnCoordsCallback)
self.get_map_value_client_map_ = dict()
self.get_local_frontier_target_pt_client_map_ = dict()
self.robot_pose_sub_ = self.create_subscription(
Point, self.robot_name_ + '/robot_pose', self.robotPoseCallback,
10)
self.robot_pose_sub_ # prevent unused variable warning
self.wfd_service_client = self.create_client(
WfdService,
self.robot_name_ + '/wfd_service',
callback_group=self.para_group)
self._action_client = ActionClient(
self, GroupCoordinator,
self.robot_name_ + '/group_coordinator_action')
self.wfd_action_client = ActionClient(self, WfdAction,
self.robot_name_ + '/wfd_action')
self.local_map_callback_lock_ = False
map_topic = ''
if self.robot_name_ == '':
map_topic = '/map'
else:
map_topic = '/' + self.robot_name_ + '/map'
self.local_map_sub_ = self.create_subscription(OccupancyGrid,
map_topic,
self.localMapCallback,
10)
# self.discover_beacon_pub_ = self.create_publisher(String, 'robot_beacon', 10)
self.debug_merge_frontiers_pub_ = self.create_publisher(
OccupancyGrid, self.robot_name_ + '/merged_frontiers_debug', 10)
self.debug_merge_map_pub_ = self.create_publisher(
OccupancyGrid, self.robot_name_ + '/merged_map_debug', 10)
self.debug_frontier_pub_ = self.create_publisher(
OccupancyGrid, self.robot_name_ + '/frontier_map_debug', 10)
self.inflated_map_pub_ = self.create_publisher(
OccupancyGrid, self.robot_name_ + '/inflated_map_debug', 10)
self.init_offset_dict_ = dict()
self.init_offset_to_current_robot_dict_ = dict()
self.declare_parameters(namespace='',
parameters=[('robot_name', None),
('peer_list', None),
('simulation_mode', None),
('tb0_init_offset', None),
('tb1_init_offset', None),
('tb2_init_offset', None),
('tb3_init_offset', None),
('tb4_init_offset', None),
('tb5_init_offset', None),
('tb6_init_offset', None),
('tb7_init_offset', None),
('tb8_init_offset', None),
('tb9_init_offset', None),
('pid', None)])
# qos_profile=qos_profile_sensor_data)
# rclpy.spin_once(self)
peer_list_param_name = 'peer_list'
param_robot_peers_ = self.get_parameter(peer_list_param_name).value
print('robot peers from param file:')
i = 0
while i < self.total_robot_num_:
self.persistent_robot_peers_.append(param_robot_peers_[i])
i = i + 1
print(self.persistent_robot_peers_)
print(len(self.persistent_robot_peers_))
self.e_util = ExploreUtil()
self.r_interface_ = RobotControlInterface(self.robot_name_)
self.r_interface_.debugPrint()
for peer in self.persistent_robot_peers_:
if peer != self.robot_name_:
self.get_map_value_client_map_[peer] = self.create_client(
GetPeerMapValueOnCoords,
peer + '/get_map_value_on_coords',
callback_group=self.para_group)
self.get_local_frontier_target_pt_client_map_[
peer] = self.create_client(
GetLocalFrontierTargetPt,
peer + '/get_local_frontier_target_pt_service',
callback_group=self.para_group)
# self.peer_interface_ = PeerInterfaceNode(self.robot_name_)
self.tic_ = 0
# self.group_coordinator_ = GroupCoordinator(self.robot_name_)
#support function update()
self.window_frontiers = None
self.window_frontiers_msg = None
self.window_frontiers_rank = None
self.peer_state_pid_list_ = None
self.going_to_target_failed_times_ = 0
self.is_leader_in_current_cluster = False
self.last_failed_frontier_pt_ = Pose()
self.is_action_finished_ = True
self.is_wfd_action_finished_ = True
self.group_action_result_pose_ = None
self.group_action_result_return_state_ = None
self.group_action_result_check_pt_list_ = None
self.group_action_result_dist_to_f_list_ = None
self.group_action_result_f_list_ = None
self.peer_track_list_ = None
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 setUp(self):
self.buffer = Buffer()
self.listener.buffer = self.buffer
def __init__(self, robot_name):
super().__init__('peer_interface_node_' + robot_name)
self.e_util = ExploreUtil()
self.robot_name_ = robot_name
self.peer_robot_registry_list_ = dict(
) # all the active robots, including current self robot
self.current_pose_local_frame_ = Pose()
self.current_pose_world_frame_ = Pose()
self.current_state_ = self.e_util.SYSTEM_INIT
self.current_target_pose_ = Pose()
#robot_registry subscriber
self.robot_registry_sub_ = self.create_subscription(
RobotRegistry, 'robot_registry', self.robotRegistryCallback, 10)
self.robot_registry_sub_ # prevent unused variable warning
#timer for getting current robot pose
# self.get_current_pose_timer_ = self.create_timer(2, self.onGetCurrentPoseTimer)
#service server for distributing current robot pose to peer robots
self.current_pose_srv = self.create_service(
GetPeerRobotPose, self.robot_name_ + '/get_current_robot_pose',
self.getCurrentRobotPoseCallback)
# self.robot_state_srv = self.create_service(GetPeerRobotState, self.robot_name_ + '/get_peer_robot_state', self.getPeerRobotStateCallback)
# self.robot_state_pid_srv = self.create_service(GetPeerRobotStatePid, self.robot_name_ + '/get_peer_robot_state_and_pid', self.getPeerRobotStatePidCallback)
timer_period = 2.5 # seconds
self.timer = self.create_timer(timer_period, self.timerCallback)
self.robottrack_publisher_ = self.create_publisher(
RobotTracks, 'robot_tracks', 10)
self.robottrack_marker_publisher_ = self.create_publisher(
Marker, self.robot_name_ + '/robot_tracks_marker', 10)
self.robot_tracks = []
self.last_track_ = Point()
self.declare_parameters(namespace='',
parameters=[('robot_name', None),
('simulation_mode', None),
('tb0_init_offset', None),
('tb1_init_offset', None),
('tb2_init_offset', None),
('tb3_init_offset', None),
('pid', None)])
self.current_robot_pid_ = self.get_parameter('pid').value
self._lock = threading.Lock()
self._tf_future = None
self._when = None
self.local_map_frame_ = self.robot_name_ + '/map'
self.local_robot_frame_ = self.robot_name_ + '/base_footprint'
self._tf_buffer = Buffer()
self._tf_listener = TransformListener(self._tf_buffer, self)
self.cmd_vel_pub_ = self.create_publisher(Twist,
robot_name + '/cmd_vel', 10)
self.navigate_to_pose_client_ = ActionClient(
self, NavigateToPose, robot_name + '/navigate_to_pose')
self.get_path_result = None
self.get_path_done_ = False
self.navigate_to_pose_state_ = self.e_util.NAVIGATION_NO_GOAL
self.init_offset_dict_ = dict()
self.init_offset_to_current_robot_dict_ = dict()
# self.getPeerRobotInitPose()
self.cluster_range_limit_ = 5.5
self.current_cluster_ = []
#priority id request client
self.pid_request_client_dict_ = dict()
#peer robot state request client
self.peer_robot_state_client_dict_ = dict()
#peer robot state_and_pid request client
self.peer_robot_state_and_pic_client_dict_ = dict()