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')
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))
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 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 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_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)
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 Robot(Node):
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)
@staticmethod
def quaternion_to_euler(quaternion):
t0 = +2.0 * (quaternion.w * quaternion.x + quaternion.y * quaternion.z)
t1 = +1.0 - 2.0 * (quaternion.x * quaternion.x +
quaternion.y * quaternion.y)
roll = atan2(t0, t1)
t2 = +2.0 * (quaternion.w * quaternion.y - quaternion.z * quaternion.x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
pitch = asin(t2)
t3 = +2.0 * (quaternion.w * quaternion.z + quaternion.x * quaternion.y)
t4 = +1.0 - 2.0 * (quaternion.y * quaternion.y +
quaternion.z * quaternion.z)
yaw = atan2(t3, t4)
return [yaw, pitch, roll]
@staticmethod
def euler_to_quaternion(roll, pitch, yaw):
qx = sin(roll / 2) * cos(pitch / 2) * cos(yaw / 2) - cos(
roll / 2) * sin(pitch / 2) * sin(yaw / 2)
qy = cos(roll / 2) * sin(pitch / 2) * cos(yaw / 2) + sin(
roll / 2) * cos(pitch / 2) * sin(yaw / 2)
qz = cos(roll / 2) * cos(pitch / 2) * sin(yaw / 2) - sin(
roll / 2) * sin(pitch / 2) * cos(yaw / 2)
qw = cos(roll / 2) * cos(pitch / 2) * cos(yaw / 2) + sin(
roll / 2) * sin(pitch / 2) * sin(yaw / 2)
return [qx, qy, qz, qw]
def get_robot_pose(self):
try:
((robot_x, robot_y, z),
robot_rot) = self.tf_buffer.lookup_transform(
"/map", "base_link", Time())
robot_yaw = self.quaternion_to_euler(robot_rot)[2]
self.robot_pose = {'x': robot_x, 'y': robot_y, 'yaw': robot_yaw}
except LookupException:
self.get_logger().info('transform not ready')
def get_map(self):
self.get_logger().info("Requesting GetMap data")
self.call = self.map_srv.call(GetMap.Request())
if self.call.done():
try:
response: GetMap.Response = self.call.result()
self.get_logger().info("GetMap data received")
response_map = response.map()
self.width = response_map.info.width
self.height = response_map.info.height
self.resolution = response_map.info.resolution
origin_yaw = self.quaternion_to_euler(
response_map.info.origin.orientation)[2]
origin_x = response_map.info.origin.position.x
origin_y = response_map.info.origin.position.y
self.map_origin = {
'x': origin_x,
'y': origin_y,
'yaw': origin_yaw
}
self.tf_timer = self.create_timer(1.0, self.get_robot_pose)
self.map_data = list(response_map.data)
for i in range(self.height):
for j in range(self.width):
self.map_data[i * self.width + j] = response_map.data[
(self.height - 1 - i) * self.width + j]
except Exception as e:
self.get_logger().info('Service call failed %r' % (e, ))
def pose_to_pix(self):
pix_x = (self.map_origin.y -
self.robot_pose.y) / self.resolution + self.height
pix_y = (self.robot_pose.x - self.map_origin.x) / self.resolution
pix_yaw = self.robot_pose.z - self.map_origin.z
return int(pix_x), int(pix_y), pix_yaw
class KFHungarianTracker(Node):
'''Use Kalman Fiter and Hungarian algorithm to track multiple dynamic obstacles
Use Hungarian algorithm to match presenting obstacles with new detection and maintain a kalman filter for each obstacle.
spawn ObstacleClass when new obstacles come and delete when they disappear for certain number of frames
Attributes:
obstacle_list: a list of ObstacleClass that currently present in the scene
sec, nanosec: timing from sensor msg
detection_sub: subscrib detection result from detection node
tracker_obstacle_pub: publish tracking obstacles with ObstacleArray
tracker_pose_pub: publish tracking obstacles with PoseArray, for rviz visualization
'''
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.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 callback(self, msg):
'''callback function for detection result'''
# update delta time
dt = (msg.header.stamp.sec - self.sec) + (msg.header.stamp.nanosec - self.nanosec) / 1e9
self.sec = msg.header.stamp.sec
self.nanosec = msg.header.stamp.nanosec
# get detection
detections = msg.obstacles
num_of_detect = len(detections)
num_of_obstacle = len(self.obstacle_list)
# kalman predict
for obj in self.obstacle_list:
obj.predict(dt)
# transform to global frame
if self.global_frame is not None:
try:
trans = self.tf_buffer.lookup_transform(self.global_frame, msg.header.frame_id, rclpy.time.Time())
msg.header.frame_id = self.global_frame
for i in range(len(detections)):
# transform position (point)
p = PointStamped()
p.point = detections[i].position
detections[i].position = do_transform_point(p, trans).point
# transform velocity (vector3)
v = Vector3Stamped()
v.vector = detections[i].velocity
detections[i].velocity = do_transform_vector3(v, trans).vector
# transform size (vector3)
s = Vector3Stamped()
s.vector = detections[i].size
detections[i].size = do_transform_vector3(s, trans).vector
except LookupException:
self.get_logger().info('fail to get tf from {} to {}'.format(msg.header.frame_id, self.global_frame))
return
# hungarian matching
cost = np.zeros((num_of_obstacle, num_of_detect))
for i in range(num_of_obstacle):
for j in range(num_of_detect):
cost[i, j] = self.obstacle_list[i].distance(detections[j])
obs_ind, det_ind = linear_sum_assignment(cost)
# filter assignment according to cost
new_obs_ind = []
new_det_ind = []
for o, d in zip(obs_ind, det_ind):
if cost[o, d] < self.cost_filter:
new_obs_ind.append(o)
new_det_ind.append(d)
obs_ind = new_obs_ind
det_ind = new_det_ind
# kalman update
for o, d in zip(obs_ind, det_ind):
self.obstacle_list[o].correct(detections[d])
# birth of new detection obstacles and death of disappear obstacle
self.birth(det_ind, num_of_detect, detections)
dead_object_list = self.death(obs_ind, num_of_obstacle)
# apply velocity and height filter
filtered_obstacle_list = []
for obs in self.obstacle_list:
obs_vel = np.linalg.norm(np.array([obs.msg.velocity.x, obs.msg.velocity.y, obs.msg.velocity.z]))
obs_height = obs.msg.position.z
if obs_vel > self.vel_filter[0] and obs_vel < self.vel_filter[1] and obs_height > self.height_filter[0] and obs_height < self.height_filter[1]:
filtered_obstacle_list.append(obs)
# construct ObstacleArray
if self.tracker_obstacle_pub.get_subscription_count() > 0:
obstacle_array = ObstacleArray()
obstacle_array.header = msg.header
track_list = []
for obs in filtered_obstacle_list:
# do not publish obstacles with low speed
track_list.append(obs.msg)
obstacle_array.obstacles = track_list
self.tracker_obstacle_pub.publish(obstacle_array)
# rviz visualization
if self.tracker_marker_pub.get_subscription_count() > 0:
marker_array = MarkerArray()
marker_list = []
# add current active obstacles
for obs in filtered_obstacle_list:
obstacle_uuid = uuid.UUID(bytes=bytes(obs.msg.uuid.uuid))
(r, g, b) = colorsys.hsv_to_rgb(obstacle_uuid.int % 360 / 360., 1., 1.) # encode id with rgb color
# make a cube
marker = Marker()
marker.header = msg.header
marker.ns = str(obstacle_uuid)
marker.id = 0
marker.type = 1 # CUBE
marker.action = 0
marker.color.a = 0.5
marker.color.r = r
marker.color.g = g
marker.color.b = b
marker.pose.position = obs.msg.position
angle = np.arctan2(obs.msg.velocity.y, obs.msg.velocity.x)
marker.pose.orientation.z = np.float(np.sin(angle / 2))
marker.pose.orientation.w = np.float(np.cos(angle / 2))
marker.scale = obs.msg.size
marker_list.append(marker)
# make an arrow
arrow = Marker()
arrow.header = msg.header
arrow.ns = str(obstacle_uuid)
arrow.id = 1
arrow.type = 0
arrow.action = 0
arrow.color.a = 1.0
arrow.color.r = r
arrow.color.g = g
arrow.color.b = b
arrow.pose.position = obs.msg.position
arrow.pose.orientation.z = np.float(np.sin(angle / 2))
arrow.pose.orientation.w = np.float(np.cos(angle / 2))
arrow.scale.x = np.linalg.norm([obs.msg.velocity.x, obs.msg.velocity.y, obs.msg.velocity.z])
arrow.scale.y = 0.05
arrow.scale.z = 0.05
marker_list.append(arrow)
# add dead obstacles to delete in rviz
for uuid in dead_object_list:
marker = Marker()
marker.header = msg.header
marker.ns = str(uuid)
marker.id = 0
marker.action = 2 # delete
arrow = Marker()
arrow.header = msg.header
arrow.ns = str(uuid)
arrow.id = 1
arrow.action = 2
marker_list.append(marker)
marker_list.append(arrow)
marker_array.markers = marker_list
self.tracker_marker_pub.publish(marker_array)
def birth(self, det_ind, num_of_detect, detections):
'''generate new ObstacleClass for detections that do not match any in current obstacle list'''
for det in range(num_of_detect):
if det not in det_ind:
obstacle = ObstacleClass(detections[det], self.top_down, self.measurement_noise_cov, self.error_cov_post, self.process_noise_cov)
self.obstacle_list.append(obstacle)
def death(self, obj_ind, num_of_obstacle):
'''count obstacles' missing frames and delete when reach threshold'''
new_object_list = []
dead_object_list = []
# for previous obstacles
for obs in range(num_of_obstacle):
if obs not in obj_ind:
self.obstacle_list[obs].dying += 1
else:
self.obstacle_list[obs].dying = 0
if self.obstacle_list[obs].dying < self.death_threshold:
new_object_list.append(self.obstacle_list[obs])
else:
obstacle_uuid = uuid.UUID(bytes=bytes(self.obstacle_list[obs].msg.uuid.uuid))
dead_object_list.append(obstacle_uuid)
# add newly born obstacles
for obs in range(num_of_obstacle, len(self.obstacle_list)):
new_object_list.append(self.obstacle_list[obs])
self.obstacle_list = new_object_list
return dead_object_list
class MultiExploreNode(Node):
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 setRobotState(self, state):
self.current_state_ = state
# self.peer_interface_.current_state_ = state
def robotPoseCallback(self, msg):
# self.current_pos_[1] = msg.y
self.current_pose_local_frame_.pose.position.x = msg.x
self.current_pose_local_frame_.pose.position.y = msg.y
self.current_pose_local_frame_.pose.position.z = 0.0
self.current_pose_local_frame_.pose.orientation.x = 0.0
self.current_pose_local_frame_.pose.orientation.y = 0.0
self.current_pose_local_frame_.pose.orientation.z = 0.0
self.current_pose_local_frame_.pose.orientation.w = 1.0
self.current_pos_ = (msg.x, msg.y)
def getLocalMapAndFrontierCompressCallback(self, request, response):
# self.get_logger().warn('{} getLocalMapAndFrontierCompressRequest'.format(self.robot_name_))
if self.inflated_local_map_ == None:
self.inflated_local_map_ = OccupancyGrid()
local_map_bytes = pickle.dumps(self.inflated_local_map_)
# self.get_logger().warn('before compressed map size:{}'.format(len(local_map_bytes)))
compressed_local_map_bytes = lzma.compress(local_map_bytes)
# self.get_logger().warn('after compressed map size:{}'.format(len(compressed_local_map_bytes)))
# print(compressed_local_map_bytes)
# response.map_compress = []
# for i in range(len(compressed_local_map_bytes)):
# response.map_compress.append(compressed_local_map_bytes[i])
response.map_compress = list(compressed_local_map_bytes)
# response.map_compress = [b'a',b'c',b'r',b'w']
response.local_frontier = self.local_frontiers_msg_
# self.get_logger().warn('send map time: {}'.format(time.time()))
return response
def getLocalMapAndFrontierCallback(self, request, response):
self.get_logger().warn('{} getLocalMapAndFrontierRequest'.format(
self.robot_name_))
if self.inflated_local_map_ == None:
self.inflated_local_map_ = OccupancyGrid()
response.map = self.inflated_local_map_
response.local_frontier = self.local_frontiers_msg_
self.get_logger().warn('send map time: {}'.format(time.time()))
return response
def getRobotCurrentPos(self):
#return True, if success,
#return False, if exception
when = rclpy.time.Time()
try:
# Suspends callback until transform becomes available
# t_0 = time.time()
transform = self._tf_buffer.lookup_transform(
self.world_frame_,
self.local_fixed_frame_,
when,
timeout=Duration(seconds=5.0))
# self.get_logger().info('Got {}'.format(repr(transform)))
self.current_pos_ = (transform.transform.translation.x,
transform.transform.translation.y)
self.current_pose_local_frame_.pose.position.x = self.current_pos_[
0]
self.current_pose_local_frame_.pose.position.y = self.current_pos_[
1]
self.current_pose_local_frame_.pose.position.z = 0.0
self.current_pose_local_frame_.pose.orientation.x = transform.transform.rotation.x
self.current_pose_local_frame_.pose.orientation.y = transform.transform.rotation.y
self.current_pose_local_frame_.pose.orientation.z = transform.transform.rotation.z
self.current_pose_local_frame_.pose.orientation.w = transform.transform.rotation.w
# t_1 = time.time()
# self.get_logger().info('(getRobotCurrentPos): robot pos:({},{}), used time:{}'.format(self.current_pos_[0], self.current_pos_[1], t_1 - t_0))
return True
except LookupException as e:
self.get_logger().error('failed to get transform {}'.format(
repr(e)))
return False
def localMapCallback(self, map_msg):
#do a window_WFD from robot's current position, get a set of new frontiers, and integrate the new found frontiers with existing self.local_frontiers_
#self.get_logger().warn('{}:localMapCallback'.format(self.tic_))
mutex = Lock()
mutex.acquire()
if self.local_map_callback_lock_ == True:
return
self.local_map_callback_lock_ = True
self.local_map_ = map_msg
#self.get_logger().warn('{}:before inflateMap'.format(self.tic_))
# self.inflated_local_map_ = self.e_util.inflateMap(self.local_map_, 5)
self.inflated_local_map_ = self.local_map_
#self.get_logger().warn('{}:after inflateMap'.format(self.tic_))
self.inflated_map_pub_.publish(self.local_map_)
self.local_map_callback_lock_ = False
self.tic_ = self.tic_ + 1
mutex.release()
def generateFrontierDebugMap(self, frontier_msg_list, current_map):
local_map_dw = current_map.info.width
local_map_dh = current_map.info.height
frontiers_index_list_w = []
frontiers_index_list_h = []
# for f_connect in local_frontiers_cell:
# for f_cell in f_connect:
# frontiers_index_list.append(f_cell[1]*local_map_dw + f_cell[0])
for f_connect_msg in frontier_msg_list:
for f_pt in f_connect_msg.frontier:
f_cell = ((int)(
(f_pt.point.x - current_map.info.origin.position.x) /
current_map.info.resolution), (int)(
(f_pt.point.y - current_map.info.origin.position.y) /
current_map.info.resolution))
frontiers_index_list_h.append(f_cell[1])
frontiers_index_list_w.append(f_cell[0])
frontier_debug_map = OccupancyGrid()
frontier_debug_map.header = copy.deepcopy(current_map.header)
frontier_debug_map.info = copy.deepcopy(current_map.info)
temp_array = np.asarray(current_map.data,
dtype=np.int8).reshape(local_map_dh,
local_map_dw)
temp_array[:] = (int)(-1)
temp_array[frontiers_index_list_h, frontiers_index_list_w] = 0
# temp_array = np.zeros((1, local_map_dw*local_map_dh), dtype=np.int8)
frontier_debug_map.data = temp_array.ravel().tolist()
# for idx in frontiers_index_list:
# if int(idx) < 0 or int(idx) > len(frontier_debug_map.data)-1:
# continue
# frontier_debug_map.data[int(idx)] = 0
# frontier_map_width = frontier_debug_map.info.width
# frontier_map_height = frontier_debug_map.info.height
return frontier_debug_map
def updateMultiHierarchicalCoordination(self):
#state of current robot: GOING_TO_TARGET, WAITING_NEW_TARGET, REACH_TARGET, FINISH_TARGET_WINDOW_NOT_DONE,
if self.current_state_ == self.e_util.SYSTEM_INIT:
#robot rotate inplace
self.r_interface_.rotateNCircles(1, 0.4)
# self.updateWindowWFD()
self.r_interface_.stopAtPlace()
#self.current_state_ = self.TEST_MERGE_MAP
self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
elif self.current_state_ == self.e_util.GOING_TO_TARGET:
self.get_logger().error('updateMulti: enter GOING_TO_TARGET')
if self.current_target_pos_ == None:
self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
return
#self.get_logger().warn('go to target:({},{}), orientation({},{},{},{})'.format(self.current_target_pos_.position.x, self.current_target_pos_.position.y, self.current_target_pos_.orientation.x, self.current_target_pos_.orientation.y, self.current_target_pos_.orientation.z, self.current_target_pos_.orientation.w))
self.r_interface_.navigateToPoseSwarmSimulationFunction(
self.current_target_pos_)
# self.getRobotCurrentPos()
# direct_length_square = (self.current_pos_[0] - self.current_target_pos_.position.x)*(self.current_pos_[0] - self.current_target_pos_.position.x) + (self.current_pos_[1] - self.current_target_pos_.position.y)*(self.current_pos_[1] - self.current_target_pos_.position.y)
# is_thread_started = False
# check_environment_thread = Thread(target=self.checkEnvironmentFunction)
# while self.r_interface_.navigate_to_pose_state_ == self.e_util.NAVIGATION_MOVING:
# # self.get_logger().error('start checking environment...')
# # self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
# # self.getRobotCurrentPos()
# current_direct_length_square = (self.current_pos_[0] - self.current_target_pos_.position.x)*(self.current_pos_[0] - self.current_target_pos_.position.x) + (self.current_pos_[1] - self.current_target_pos_.position.y)*(self.current_pos_[1] - self.current_target_pos_.position.y)
# # self.get_logger().warn("navigating to target {},{}".format(self.current_target_pos_.position.x, self.current_target_pos_.position.y ))
# # current_direct_length_square = 10.0
# if current_direct_length_square < 2.0*2.0:
# if is_thread_started == False:
# check_environment_thread.start()
# is_thread_started = True
# # return
# pass
while self.r_interface_.navigate_to_pose_state_ == self.e_util.NAVIGATION_MOVING:
pass
if self.r_interface_.navigate_to_pose_state_ == self.e_util.NAVIGATION_DONE:
self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
elif self.r_interface_.navigate_to_pose_state_ == self.e_util.NAVIGATION_FAILED:
self.get_logger().error('NAVIGATION_FAILED')
current_target_cell = ((int)(
(self.current_target_pos_.position.x -
self.inflated_local_map_.info.origin.position.x) /
self.inflated_local_map_.info.resolution), (int)(
(self.current_target_pos_.position.y -
self.inflated_local_map_.info.origin.position.y) /
self.inflated_local_map_.info.resolution))
# self.getRobotCurrentPos()
# current_cell = ((int)((self.current_pos_[0] - self.inflated_local_map_.info.origin.position.x) / self.inflated_local_map_.info.resolution) , (int)((self.current_pos_[1] - self.inflated_local_map_.info.origin.position.y) / self.inflated_local_map_.info.resolution))
updated_cell = self.e_util.getFreeNeighborRandom(
current_target_cell, self.inflated_local_map_, 30, 50)
if updated_cell == None:
self.get_logger().warn(
'updated_cell is None, CHECK_ENVIRONMENT now!!!!!')
self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
return
updated_target_pt = (
updated_cell[0] * self.inflated_local_map_.info.resolution
+ self.inflated_local_map_.info.origin.position.x,
updated_cell[1] * self.inflated_local_map_.info.resolution
+ self.inflated_local_map_.info.origin.position.y)
self.current_target_pos_.position.x = updated_target_pt[0]
self.current_target_pos_.position.y = updated_target_pt[1]
if self.previous_state_ != self.e_util.GOING_TO_TARGET:
self.get_logger().error(
'going_to_target_failed_times_ = 000000000000')
self.going_to_target_failed_times_ = 0
else:
self.going_to_target_failed_times_ += 1
self.get_logger().error(
'going_to_target_failed_times_ = {}{}{}{}{}'.format(
self.going_to_target_failed_times_,
self.going_to_target_failed_times_,
self.going_to_target_failed_times_,
self.going_to_target_failed_times_,
self.going_to_target_failed_times_))
self.previous_state_ = self.current_state_
if self.going_to_target_failed_times_ > 10:
self.get_logger().warn(
'going_to_target_failed_times_ > 10')
self.get_logger().warn(
'going_to_target_failed_times_ > 10')
self.get_logger().warn(
'going_to_target_failed_times_ > 10')
self.get_logger().warn(
'going_to_target_failed_times_ > 10')
self.get_logger().warn(
'going_to_target_failed_times_ > 10')
self.last_failed_frontier_pt_ = self.current_target_pos_
self.going_to_target_failed_times_ = 0
self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
else:
self.get_logger().error(
'retry same target again!!!!!!!!!!!!!!!!')
self.setRobotState(self.e_util.GOING_TO_TARGET)
# if is_thread_started == True:
# check_environment_thread.join()
# self.current_target_pos_ = self.next_target_pos_
# is_thread_started == False
# self.setRobotState(self.e_util.GOING_TO_TARGET)
self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
elif self.current_state_ == self.e_util.CHECK_ENVIRONMENT:
self.get_logger().error('Enter CHECK_ENVIRONMENT1')
#check current window_frontiers, if window_frontiers is empty, then FINISH_TARGET_WINDOW_DONE
#if window_frontiers is not empty, then FINISH_TARGET_WINDOW_NOT_DONE
wfd_response_future = self.send_wfd_service_request()
# self.send_wfd_action_goal()
# while self.is_wfd_action_finished_ != True:
# pass
while not wfd_response_future.done():
pass
wfd_response = wfd_response_future.result()
# self.get_logger().error('trying to get response from wfd service')
# rclpy.spin_once(self)
# wfd_response = wfd_response_future.result()
self.get_logger().error('got response from wfd service')
self.local_frontiers_msg_ = wfd_response.local_frontiers
self.window_frontiers_msg = wfd_response.window_frontiers
self.window_frontiers_rank = wfd_response.window_frontiers_rank
# self.window_frontiers, self.window_frontiers_msg, self.window_frontiers_rank = self.updateLocalFrontiersUsingWindowWFD()
# if self.window_frontiers == -1 or self.window_frontiers == -2:
# self.get_logger().error('(update.CHECK_ENVIRONMENT) failed getting WindowFrontiers, check robot, something is wrong')
# self.current_state_ = self.e_util.CHECK_ENVIRONMENT
# else:
self.send_group_coordinator_goal()
while self.is_action_finished_ != True:
pass
if self.group_action_result_return_state_ == 1:
self.current_target_pos_ = self.group_action_result_pose_
elif self.group_action_result_return_state_ == 2:
#check window_frontier_pt, if 1), all window_f_pt covered, then go to the local_f_pt that is furthest from peers; 2) not all window_f_pt covered,
# go to closest uncovered window_f_pt.
all_window_f_covered_by_peers = True
f_pt_index_to_peer_value_map = self.send_get_map_values_request(
self.group_action_result_check_pt_list_)
uncovered_f_pt_index_list = []
for f_pt_index in f_pt_index_to_peer_value_map:
is_covered = False
for f_pt_value in f_pt_index_to_peer_value_map[f_pt_index]:
if f_pt_value != -1 and f_pt_value < 80:
is_covered = True
break
if is_covered == False:
uncovered_f_pt_index_list.append(f_pt_index)
if len(uncovered_f_pt_index_list) > 0:
all_window_f_covered_by_peers = False
#go to closest uncovered window_f_pt
closest_dist = 10000000
closest_index = 0
for index in range(
len(self.group_action_result_dist_to_f_list_)):
if index in uncovered_f_pt_index_list:
if self.group_action_result_dist_to_f_list_[
index] < closest_dist:
closest_dist = self.group_action_result_dist_to_f_list_[
index]
closest_index = index
closest_window_f_pt = Pose()
closest_window_f_pt.position.x = self.group_action_result_f_list_[
closest_index].x
closest_window_f_pt.position.y = self.group_action_result_f_list_[
closest_index].y
self.current_target_pos_ = closest_window_f_pt
else:
all_window_f_covered_by_peers = True
#go to the furthest local_f_pt from peers
self.current_target_pos_ = self.group_action_result_pose_
elif self.group_action_result_return_state_ == 3:
#check local_frontier_pt, if 1), all local_f_pt covered, then merge map, 2), not all local_f_pt covered, go to closest local_f_pt
all_local_f_covered_by_peers = True
f_pt_index_to_peer_value_map = self.send_get_map_values_request(
self.group_action_result_check_pt_list_)
uncovered_f_pt_index_list = []
for f_pt_index in f_pt_index_to_peer_value_map:
is_covered = False
for f_pt_value in f_pt_index_to_peer_value_map[f_pt_index]:
if f_pt_value != -1 and f_pt_value < 80:
is_covered = True
break
if is_covered == False:
uncovered_f_pt_index_list.append(f_pt_index)
if len(uncovered_f_pt_index_list) > 0:
all_local_f_covered_by_peers = False
#go to closest uncovered local_f_pt, doesn't another action request
closest_dist = 10000000
closest_index = 0
for index in range(
len(self.group_action_result_dist_to_f_list_)):
if index in uncovered_f_pt_index_list:
if self.group_action_result_dist_to_f_list_[
index] < closest_dist:
closest_dist = self.group_action_result_dist_to_f_list_[
index]
closest_index = index
closest_window_f_pt = Pose()
closest_window_f_pt.position.x = self.group_action_result_f_list_[
closest_index].x
closest_window_f_pt.position.y = self.group_action_result_f_list_[
closest_index].y
self.current_target_pos_ = closest_window_f_pt
else:
all_local_f_covered_by_peers = True
#request local_frontiers target points from peers, find the one that is furthest from peer tracks
local_frontier_target_pt_list = self.send_get_local_frontiers_target_value_request(
)
largest_dist = -1.0
furthest_target_pt = Point()
if len(local_frontier_target_pt_list) > 0:
for target_pt in local_frontier_target_pt_list:
min_dist = 10000000000
for track in self.peer_track_list_:
dist = (target_pt.x -
track.x) * (target_pt.x - track.x) + (
target_pt.y -
track.y) * (target_pt.y - track.y)
if dist < min_dist:
min_dist = dist
if min_dist > largest_dist:
largest_dist = min_dist
furthest_target_pt = target_pt
target_pose = Pose()
target_pose.position.x = furthest_target_pt.x
target_pose.position.y = furthest_target_pt.y
self.current_target_pos_ = target_pose
else:
self.current_target_pos_ = None
#only case that requires another action request
#send another action request to merge map and find closest merged_f_pt
# self.group_coordinator_.setPeerInfo(self.persistent_robot_peers_, peer_pose_dict, cluster_list, cluster_pose_dict, self.window_frontiers, self.window_frontiers_rank, self.local_frontiers_, self.local_frontiers_msg_, self.inflated_local_map_, self.peer_interface_.init_offset_dict_, self.last_failed_frontier_pt_)
# self.current_target_pos_ = self.group_coordinator_.hierarchicalCoordinationAssignment()
if self.current_target_pos_ == None:
self.get_logger().error(
'finish local_frontiers, done for current robot')
self.setRobotState(self.e_util.FINISH_TASK)
else:
self.previous_state_ = self.e_util.CHECK_ENVIRONMENT
self.setRobotState(self.e_util.GOING_TO_TARGET)
# no window frontiers available, find target frontier from self.local_frontiers_msg_, considering distance from peer robot_tracks
elif self.current_state_ == self.e_util.FINISH_TASK:
self.r_interface_.rotateNCircles(1, 0.4)
self.get_logger().error(
'finish local_frontiers, done for current robot')
def send_group_coordinator_goal(self):
self.get_logger().error('Enter send_group_coordinator_goal')
self._action_client.wait_for_server()
while self.is_action_finished_ == False:
pass
self.get_logger().error('send_group_coordinator_goal ready')
self.is_action_finished_ = False
goal_msg = GroupCoordinator.Goal()
for peer in self.persistent_robot_peers_:
peer_msg = String()
if peer != self.robot_name_:
peer_msg.data = peer
goal_msg.peer_list.append(peer_msg)
# if self.getRobotCurrentPos():
# else:
# self.get_logger().error('(send_group_coordinator_goal) fail to get robot current pose')
# return
goal_msg.robot_pose_local_frame = self.current_pose_local_frame_
goal_msg.window_frontiers = self.window_frontiers_msg
goal_msg.window_frontiers_rank = self.window_frontiers_rank
goal_msg.local_frontiers = self.local_frontiers_msg_
goal_msg.local_inflated_map = self.inflated_local_map_
goal_msg.last_failed_frontier_pt.pose = self.last_failed_frontier_pt_
# goal_uuid = 0
# if self.robot_name_ == 'tb0':
# goal_uuid = 0
# elif self.robot_name_ == 'tb1':
# goal_uuid = 1
self._send_goal_future = self._action_client.send_goal_async(
goal_msg,
feedback_callback=self.feedback_group_coordinator_callback)
self._send_goal_future.add_done_callback(self.goal_response_callback)
def feedback_group_coordinator_callback(self, feedback):
pass
def goal_response_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().error('group coordinator goal rejected')
return
self.get_logger().warn('group coordinator goal accepted')
self._get_result_future = goal_handle.get_result_async()
self._get_result_future.add_done_callback(self.get_result_callback)
def get_result_callback(self, future):
result = future.result().result
status = future.result().status
if status == GoalStatus.STATUS_SUCCEEDED:
self.get_logger().info('Goal succeeded!')
self.group_action_result_pose_ = result.current_target_pose
self.group_action_result_check_pt_list_ = result.check_pt_list
self.group_action_result_return_state_ = result.return_state
self.group_action_result_dist_to_f_list_ = result.dist_to_f_list
self.group_action_result_f_list_ = result.f_list
self.peer_track_list_ = result.peer_track_list
self.is_action_finished_ = True
else:
self.get_logger().error(
'Goal failed with status: {}'.format(status))
def send_wfd_service_request(self):
while not self.wfd_service_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('wfd servie not available,wait')
req = WfdService.Request()
# if self.getRobotCurrentPos():
# self.get_logger().error('Enter send_wfd_service_request')
req.robot_pose_local_frame = self.current_pose_local_frame_.pose
req.local_frontiers = self.local_frontiers_msg_
req.local_inflated_map = self.inflated_local_map_
return self.wfd_service_client.call_async(req)
def send_get_map_values_request(self, check_pt_list):
f_pt_index_to_peer_value_map = dict()
self.get_logger().warn('send_get_map_values_request')
for peer in self.persistent_robot_peers_:
if peer != self.robot_name_:
while not self.get_map_value_client_map_[
peer].wait_for_service(timeout_sec=1.0):
pass
req = GetPeerMapValueOnCoords.Request()
req.query_pt_list = check_pt_list
print("request " + peer)
self.get_logger().info('before')
res_future = self.get_map_value_client_map_[peer].call_async(
req)
while not res_future.done():
pass
res = res_future.result()
self.get_logger().info('after')
value_list_result = res.pt_value_list
for v_index in range(len(check_pt_list)):
if v_index in f_pt_index_to_peer_value_map:
f_pt_index_to_peer_value_map[v_index].append(
value_list_result[v_index])
else:
f_pt_index_to_peer_value_map[v_index] = []
f_pt_index_to_peer_value_map[v_index].append(
value_list_result[v_index])
return f_pt_index_to_peer_value_map
def send_get_local_frontiers_target_value_request(self):
local_frontier_target_pt_list = []
for peer in self.persistent_robot_peers_:
if peer != self.robot_name_:
while not self.get_local_frontier_target_pt_client_map_[
peer].wait_for_service(timeout_sec=1.0):
pass
req = GetLocalFrontierTargetPt.Request()
self.get_logger().info('before')
res_future = self.get_local_frontier_target_pt_client_map_[
peer].call_async(req)
self.get_logger().info('after')
while not res_future.done():
pass
response = res_future.result()
target_pt_list = response.local_frontier_target_pt
local_frontier_target_pt_list.extend(target_pt_list)
return local_frontier_target_pt_list
def send_wfd_action_goal(self):
self.wfd_action_client.wait_for_server()
while self.is_wfd_action_finished_ == False:
pass
self.is_wfd_action_finished_ = False
goal_msg = WfdAction.Goal()
goal_msg.local_frontiers = self.local_frontiers_msg_
# if self.getRobotCurrentPos():
# else:
# self.get_logger().error('(send_wfd_goal) fail to get robot current pose')
# return
goal_msg.robot_pose_local_frame = self.current_pose_local_frame_.pose
goal_msg.local_inflated_map = self.inflated_local_map_
self._send_wfd_goal_future = self.wfd_action_client.send_goal_async(
goal_msg, feedback_callback=self.feedback_wfd_action_callback)
self._send_wfd_goal_future.add_done_callback(
self.wfd_goal_response_callback)
def feedback_wfd_action_callback(self, feedback):
pass
def wfd_goal_response_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().error('wfd goal rejected')
return
self.get_logger().warn('wfd goal accepted')
self._get_wfd_result_future = goal_handle.get_result_async()
self._get_wfd_result_future.add_done_callback(
self.get_wfd_result_callback)
def get_wfd_result_callback(self, future):
result = future.result().result
status = future.result().status
if status == GoalStatus.STATUS_SUCCEEDED:
self.get_logger().info('Goal succeeded!')
self.local_frontiers_msg_ = result.local_frontiers
self.window_frontiers_msg = result.window_frontiers
self.window_frontiers_rank = result.window_frontiers_rank
# self.window_frontiers_.clear()
# for wf_msg in self.window_frontiers_msg:
self.is_wfd_action_finished_ = True
else:
self.get_logger().error(
'Goal failed with status: {}'.format(status))
def checkEnvironmentFunction(self):
# self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
# self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
# self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
self.get_logger().error('Enter CHECK_ENVIRONMENT_THREAD')
# self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
# self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
#check current window_frontiers, if window_frontiers is empty, then FINISH_TARGET_WINDOW_DONE
#if window_frontiers is not empty, then FINISH_TARGET_WINDOW_NOT_DONE
# self.send_wfd_action_goal()
# while self.is_wfd_action_finished_ != True:
# pass
wfd_response_future = self.send_wfd_service_request()
self.get_logger().error('got response from wfd service')
# self.send_wfd_action_goal()
# while self.is_wfd_action_finished_ != True:
# pass
while not wfd_response_future.done():
pass
wfd_response = wfd_response_future.result()
self.local_frontiers_msg_ = wfd_response.local_frontiers
self.window_frontiers_msg = wfd_response.window_frontiers
self.window_frontiers_rank = wfd_response.window_frontiers_rank
# self.window_frontiers, self.window_frontiers_msg, self.window_frontiers_rank = self.updateLocalFrontiersUsingWindowWFD()
# if self.window_frontiers == -1 or self.window_frontiers == -2:
# self.get_logger().error('(update.CHECK_ENVIRONMENT) failed getting WindowFrontiers, check robot, something is wrong')
# #self.current_state_ = self.e_util.CHECK_ENVIRONMENT
# else:
self.send_group_coordinator_goal()
while self.is_action_finished_ != True:
pass
self.get_logger().error('get result from group_coordinator_goal')
if self.group_action_result_return_state_ == 1:
self.get_logger().error('state 1')
# self.current_target_pos_ = self.group_action_result_pose_
self.next_target_pos_ = self.group_action_result_pose_
elif self.group_action_result_return_state_ == 2:
self.get_logger().error('state 2')
#check window_frontier_pt, if 1), all window_f_pt covered, then go to the local_f_pt that is furthest from peers; 2) not all window_f_pt covered,
# go to closest uncovered window_f_pt.
all_window_f_covered_by_peers = True
f_pt_index_to_peer_value_map = self.send_get_map_values_request(
self.group_action_result_check_pt_list_)
uncovered_f_pt_index_list = []
for f_pt_index in f_pt_index_to_peer_value_map:
is_covered = False
for f_pt_value in f_pt_index_to_peer_value_map[f_pt_index]:
if f_pt_value != -1 and f_pt_value < 80:
is_covered = True
break
if is_covered == False:
uncovered_f_pt_index_list.append(f_pt_index)
if len(uncovered_f_pt_index_list) > 0:
all_window_f_covered_by_peers = False
#go to closest uncovered window_f_pt
closest_dist = 10000000
closest_index = 0
for index in range(
len(self.group_action_result_dist_to_f_list_)):
if index in uncovered_f_pt_index_list:
if self.group_action_result_dist_to_f_list_[
index] < closest_dist:
closest_dist = self.group_action_result_dist_to_f_list_[
index]
closest_index = index
closest_window_f_pt = Pose()
closest_window_f_pt.position.x = self.group_action_result_f_list_[
closest_index].x
closest_window_f_pt.position.y = self.group_action_result_f_list_[
closest_index].y
self.next_target_pos_ = closest_window_f_pt
else:
all_window_f_covered_by_peers = True
#go to the furthest local_f_pt from peers
self.next_target_pos_ = self.group_action_result_pose_
elif self.group_action_result_return_state_ == 3:
self.get_logger().error('state 3')
#check local_frontier_pt, if 1), all local_f_pt covered, then merge map, 2), not all local_f_pt covered, go to closest local_f_pt
all_local_f_covered_by_peers = True
f_pt_index_to_peer_value_map = self.send_get_map_values_request(
self.group_action_result_check_pt_list_)
uncovered_f_pt_index_list = []
for f_pt_index in f_pt_index_to_peer_value_map:
is_covered = False
for f_pt_value in f_pt_index_to_peer_value_map[f_pt_index]:
if f_pt_value != -1 and f_pt_value < 80:
is_covered = True
break
if is_covered == False:
uncovered_f_pt_index_list.append(f_pt_index)
if len(uncovered_f_pt_index_list) > 0:
all_local_f_covered_by_peers = False
#go to closest uncovered local_f_pt, doesn't another action request
closest_dist = 10000000
closest_index = 0
for index in range(
len(self.group_action_result_dist_to_f_list_)):
if index in uncovered_f_pt_index_list:
if self.group_action_result_dist_to_f_list_[
index] < closest_dist:
closest_dist = self.group_action_result_dist_to_f_list_[
index]
closest_index = index
closest_window_f_pt = Pose()
closest_window_f_pt.position.x = self.group_action_result_f_list_[
closest_index].x
closest_window_f_pt.position.y = self.group_action_result_f_list_[
closest_index].y
self.next_target_pos_ = closest_window_f_pt
else:
all_local_f_covered_by_peers = True
local_frontier_target_pt_list = self.send_get_local_frontiers_target_value_request(
)
largest_dist = -1.0
furthest_target_pt = Point()
if len(local_frontier_target_pt_list) > 0:
for target_pt in local_frontier_target_pt_list:
min_dist = 10000000000
for track in self.peer_track_list_:
dist = (target_pt.x -
track.x) * (target_pt.x - track.x) + (
target_pt.y - track.y) * (target_pt.y -
track.y)
if dist < min_dist:
min_dist = dist
if min_dist > largest_dist:
largest_dist = min_dist
furthest_target_pt = target_pt
target_pose = Pose()
target_pose.position.x = furthest_target_pt.x
target_pose.position.y = furthest_target_pt.y
self.next_target_pos_ = target_pose
else:
self.next_target_pos_ = None
# self.get_logger().error('done ,rest......')
#only case that requires another action request
#send another action request to merge map and find closest merged_f_pt
# self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
# self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
self.get_logger().error(
'EXIT CHECK_ENVIRONMENT_THREAD, next target:{},{}'.format(
self.next_target_pos_.position.x,
self.next_target_pos_.position.y))
class RobotTrackPublisherNode(Node):
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 getPeerRobotInitPose(self):
for robot in self.persistent_robot_peers_:
param_name = robot + '_init_offset'
init_offset_param = self.get_parameter(param_name)
init_offset = init_offset_param.value
if robot not in self.init_offset_dict_:
self.init_offset_dict_[robot] = Pose()
self.init_offset_dict_[robot].position.x = init_offset[0]
self.init_offset_dict_[robot].position.y = init_offset[1]
self.init_offset_dict_[robot].position.z = init_offset[2]
self.init_offset_dict_[robot].orientation.x = init_offset[3]
self.init_offset_dict_[robot].orientation.y = init_offset[4]
self.init_offset_dict_[robot].orientation.z = init_offset[5]
self.init_offset_dict_[robot].orientation.w = init_offset[6]
def robotPoseCallback(self, msg):
self.current_pose_local_frame_.pose.position.x = msg.x
self.current_pose_local_frame_.pose.position.y = msg.y
self.current_pose_local_frame_.pose.position.z = 0.0
self.current_pose_local_frame_.pose.orientation.x = 0.0
self.current_pose_local_frame_.pose.orientation.y = 0.0
self.current_pose_local_frame_.pose.orientation.z = 0.0
self.current_pose_local_frame_.pose.orientation.w = 1.0
def timer_callback(self):
track = Point()
if self.mode_ == "real_robot":
self.getRobotCurrentPos()
track.x = self.current_pos_[0] + self.init_offset_dict_[
self.robot_name_].position.x
track.y = self.current_pos_[1] + self.init_offset_dict_[
self.robot_name_].position.y
elif self.mode_ == "swarm_simulation":
track.x = self.current_pose_local_frame_.pose.position.x
track.y = self.current_pose_local_frame_.pose.position.y
msg = RobotTrack()
msg.robot_name.data = self.robot_name_
msg.robot_track = track
self.publisher_.publish(msg)
# self.get_logger().info('Publishing: "%s"' % msg.robot_name.data)
def getRobotCurrentPos(self):
#return True, if success,
#return False, if exception
when = rclpy.time.Time()
try:
# Suspends callback until transform becomes available
# t_0 = time.time()
transform = self._tf_buffer.lookup_transform(
self.world_frame_,
self.local_fixed_frame_,
when,
timeout=Duration(seconds=5.0))
# self.get_logger().info('Got {}'.format(repr(transform)))
self.current_pos_ = (transform.transform.translation.x,
transform.transform.translation.y)
self.current_pose_local_frame_.pose.position.x = self.current_pos_[
0]
self.current_pose_local_frame_.pose.position.y = self.current_pos_[
1]
self.current_pose_local_frame_.pose.position.z = 0.0
self.current_pose_local_frame_.pose.orientation.x = transform.transform.rotation.x
self.current_pose_local_frame_.pose.orientation.y = transform.transform.rotation.y
self.current_pose_local_frame_.pose.orientation.z = transform.transform.rotation.z
self.current_pose_local_frame_.pose.orientation.w = transform.transform.rotation.w
# t_1 = time.time()
# self.get_logger().info('(getRobotCurrentPos): robot pos:({},{}), used time:{}'.format(self.current_pos_[0], self.current_pos_[1], t_1 - t_0))
return True
except LookupException as e:
self.get_logger().error('failed to get transform {}'.format(
repr(e)))
return False
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()
class PeerInterfaceNode(Node):
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()
def publishRobotTrackMarker(self, robot_name, robot_tracks):
m = Marker()
m.header = Header()
m.header.stamp = self.get_clock().now().to_msg()
m.header.frame_id = "world"
m.id = 0
m.type = 7
m.action = 0
m.scale.x = 0.5
m.scale.y = 0.5
m.scale.z = 0.5
if robot_name == 'tb0':
m.color.r = 0.0
m.color.g = 1.0
m.color.b = 0.0
m.color.a = 1.0
elif robot_name == 'tb1':
m.color.r = 1.0
m.color.g = 0.0
m.color.b = 0.0
m.color.a = 1.0
elif robot_name == 'tb2':
m.color.r = 0.0
m.color.g = 1.0
m.color.b = 1.0
m.color.a = 1.0
elif robot_name == 'tb3':
m.color.r = 0.0
m.color.g = 0.0
m.color.b = 1.0
m.color.a = 1.0
m.points = robot_tracks
self.robottrack_marker_publisher_.publish(m)
def timerCallback(self):
msg = RobotTracks()
msg.robot_name.data = self.robot_name_
if self.getRobotCurrentPose():
curr_pt = Point()
curr_pt.x = self.current_pose_world_frame_.position.x
curr_pt.y = self.current_pose_world_frame_.position.y
curr_pt.z = 0.0
if (curr_pt.x -
self.last_track_.x) * (curr_pt.x - self.last_track_.x) + (
curr_pt.y - self.last_track_.y) * (
curr_pt.y - self.last_track_.y) > 1.5 * 1.5:
self.robot_tracks.append(curr_pt)
msg.robot_tracks = self.robot_tracks
self.robottrack_publisher_.publish(msg)
self.publishRobotTrackMarker(self.robot_name_, msg.robot_tracks)
self.last_track_ = curr_pt
else:
self.get_logger().error(
"timer_callback():getRobotCurrentPose() failed, something is wrong"
)
return
# self.get_logger().error('Publishing {}''s RobotTracks'.format(self.robot_name_))
# self.get_logger().warn('******************************************')
# self.get_logger().warn('******************************************')
# self.get_logger().warn('******************************************')
# self.get_logger().warn('******************************************')
# self.get_logger().warn('******************************************')
def getCurrentRobotPoseCallback(self, request, response):
get_robot_current_pose = False
while not get_robot_current_pose:
if self.getRobotCurrentPose():
response.robot_pose = self.current_pose_local_frame_
get_robot_current_pose = True
self.get_logger().warn('{} getCurrentRobotPoseCallback'.format(
self.robot_name_))
return response
def setCurrentTargetPose(self, target_pose_in_current_frame):
self.current_target_pose_ = target_pose_in_current_frame
def getPeerRobotStateCallback(self, request, response):
get_robot_current_pose = False
while not get_robot_current_pose:
if self.getRobotCurrentPose():
self.current_pose_world_frame_ = self.current_pose_local_frame_
self.current_pose_world_frame_.position.x = self.current_pose_local_frame_.position.x + self.init_offset_dict_[
self.robot_name_].position.x
self.current_pose_world_frame_.position.y = self.current_pose_local_frame_.position.y + self.init_offset_dict_[
self.robot_name_].position.y
self.current_pose_world_frame_.position.z = self.current_pose_local_frame_.position.z + self.init_offset_dict_[
self.robot_name_].position.z
get_robot_current_pose = True
self.get_logger().warn(
'{} getPeerRobotStateCallback, robot_pose:{},{}'.format(
self.robot_name_, self.current_pose_world_frame_.position.x,
self.current_pose_world_frame_.position.y))
response.robot_state = RobotState()
response.robot_state.robot_name.data = self.robot_name_
response.robot_state.robot_pose_world_frame = self.current_pose_world_frame_
response.robot_state.battery_level = 100.0
response.robot_state.current_state = self.current_state_
response.robot_state.current_target_pose = self.current_target_pose_
response.robot_state.pid = self.current_robot_pid_
return response
def getPeerRobotStatePidCallback(self, request, response):
self.get_logger().warn('{} getPeerRobotStatePidCallback'.format(
self.robot_name_))
response.robot_state_and_pid = RobotStatePid()
response.robot_state_and_pid.robot_name.data = self.robot_name_
response.robot_state_and_pid.current_state = self.current_state_
response.robot_state_and_pid.pid = self.current_robot_pid_
return response
def robotRegistryCallback(self, msg):
peer_name = msg.robot_name.data
# if peer_name == self.robot_name_:
# return
# self.get_logger().info('robot "%s" registered' % msg.robot_name.data)
seconds = msg.header.stamp.sec
nanoseconds = msg.header.stamp.nanosec
if peer_name in self.peer_robot_registry_list_:
self.peer_robot_registry_list_[
peer_name] = seconds * 1000000000 + nanoseconds
else:
self.peer_robot_registry_list_[
peer_name] = seconds * 1000000000 + nanoseconds
self.updateRobotRegistry()
def updateRobotRegistry(self):
now = self.get_clock().now().nanoseconds()
for robot in self.peer_robot_registry_list_:
if now - self.peer_robot_registry_list_[robot] > 5000000000:
del self.peer_robot_registry_list_[robot]
def on_tf_ready(self, future):
with self._lock:
self._tf_future = None
if future.result():
try:
transform = self._tf_buffer.lookup_transform(
self.local_map_frame_, self.local_robot_frame_,
self._when)
self.current_pose_local_frame_.position.x = transform.transform.translation.x
self.current_pose_local_frame_.position.y = transform.transform.translation.y
self.current_pose_local_frame_.position.z = transform.transform.translation.z
self.current_pose_local_frame_.orientation.x = transform.transform.rotation.x
self.current_pose_local_frame_.orientation.y = transform.transform.rotation.y
self.current_pose_local_frame_.orientation.z = transform.transform.rotation.z
self.current_pose_local_frame_.orientation.w = transform.transform.rotation.w
self.current_pose_world_frame_.position.x = self.current_pose_local_frame_.position.x + self.init_offset_dict_[
self.robot_name_].position.x
self.current_pose_world_frame_.position.y = self.current_pose_local_frame_.position.y + self.init_offset_dict_[
self.robot_name_].position.y
self.current_pose_world_frame_.position.z = self.current_pose_local_frame_.position.z + self.init_offset_dict_[
self.robot_name_].position.z
except LookupException:
self.get_logger().info('transform no longer available')
else:
self.get_logger().info('Got transform')
def onGetCurrentPoseTimer(self):
if self._tf_future:
self.get_logger().warn('Still waiting for transform')
return
with self._lock:
# transform = self._tf_buffer.lookup_transform(self.local_map_frame_, self.local_robot_frame_, when,timeout=Duration(seconds=5.0))
self._tf_future = self._tf_buffer.wait_for_transform_async(
self.local_map_frame_, self.local_robot_frame_, self._when)
self._tf_future.add_done_callback(self.on_tf_ready)
self.get_logger().info(
'Waiting for transform from {} to {}'.format(
self.local_robot_frame_, self.local_map_frame_))
def getRobotCurrentPose(self):
#return True, if success,
#return False, if exception
when = rclpy.time.Time()
try:
# Suspends callback until transform becomes available
t_0 = time.time()
transform = self._tf_buffer.lookup_transform(
self.local_map_frame_,
self.local_robot_frame_,
when,
timeout=Duration(seconds=5.0))
# self.get_logger().info('Got {}'.format(repr(transform)))
self.current_pose_local_frame_.position.x = transform.transform.translation.x
self.current_pose_local_frame_.position.y = transform.transform.translation.y
self.current_pose_local_frame_.position.z = transform.transform.translation.z
self.current_pose_local_frame_.orientation.x = transform.transform.rotation.x
self.current_pose_local_frame_.orientation.y = transform.transform.rotation.y
self.current_pose_local_frame_.orientation.z = transform.transform.rotation.z
self.current_pose_local_frame_.orientation.w = transform.transform.rotation.w
self.current_pose_world_frame_.position.x = self.current_pose_local_frame_.position.x + self.init_offset_dict_[
self.robot_name_].position.x
self.current_pose_world_frame_.position.y = self.current_pose_local_frame_.position.y + self.init_offset_dict_[
self.robot_name_].position.y
self.current_pose_world_frame_.position.z = self.current_pose_local_frame_.position.z + self.init_offset_dict_[
self.robot_name_].position.z
t_1 = time.time()
# self.get_logger().info('(getRobotCurrentPos): robot pos:({},{}), used time:{}'.format(self.current_pose_local_frame_.position.x, self.current_pose_local_frame_.position.y, t_1 - t_0))
return True
except LookupException as e:
self.get_logger().error('failed to get transform {}'.format(
repr(e)))
return False
#self.init_offset_dict_ : relative position of peer robot's fixed frame to the 'world' frame, not relative to the current robot frame
def getPeerRobotInitPose(self):
for robot in self.peer_robot_registry_list_:
param_name = robot + '_init_offset'
init_offset_param = self.get_parameter(param_name)
init_offset = init_offset_param.value
if robot not in self.init_offset_dict_:
self.init_offset_dict_[robot] = Pose()
self.init_offset_dict_[robot].position.x = init_offset[0]
self.init_offset_dict_[robot].position.y = init_offset[1]
self.init_offset_dict_[robot].position.z = init_offset[2]
self.init_offset_dict_[robot].orientation.x = init_offset[3]
self.init_offset_dict_[robot].orientation.y = init_offset[4]
self.init_offset_dict_[robot].orientation.z = init_offset[5]
self.init_offset_dict_[robot].orientation.w = init_offset[6]
current_robot_offset_world_pose = self.init_offset_dict_[
self.robot_name_]
for peer in self.peer_robot_registry_list_:
if peer == self.robot_name_:
self.init_offset_to_current_robot_dict_[peer] = Pose()
self.init_offset_to_current_robot_dict_[peer].position.x = 0.0
self.init_offset_to_current_robot_dict_[peer].position.y = 0.0
self.init_offset_to_current_robot_dict_[peer].position.z = 0.0
self.init_offset_to_current_robot_dict_[
peer].orientation.x = 0.0
self.init_offset_to_current_robot_dict_[
peer].orientation.y = 0.0
self.init_offset_to_current_robot_dict_[
peer].orientation.z = 0.0
self.init_offset_to_current_robot_dict_[
peer].orientation.w = 1.0
else:
self.init_offset_to_current_robot_dict_[peer] = Pose()
self.init_offset_to_current_robot_dict_[
peer].position.x = self.init_offset_dict_[
peer].position.x - current_robot_offset_world_pose.position.x
self.init_offset_to_current_robot_dict_[
peer].position.y = self.init_offset_dict_[
peer].position.y - current_robot_offset_world_pose.position.y
self.init_offset_to_current_robot_dict_[
peer].position.z = self.init_offset_dict_[
peer].position.z - current_robot_offset_world_pose.position.z
self.init_offset_to_current_robot_dict_[
peer].orientation.x = 0.0
self.init_offset_to_current_robot_dict_[
peer].orientation.y = 0.0
self.init_offset_to_current_robot_dict_[
peer].orientation.z = 0.0
self.init_offset_to_current_robot_dict_[
peer].orientation.w = 1.0
#peer_list: if given, the list of robot names to request robot state information from, if None, request from self.peer_robot_registry_list_ (all the active robots)
def getPeerRobotStateFunction(self, peer_list=None):
service_client_dict = dict()
service_response_future = dict()
peer_robot_state_dict = dict()
peer_robot_state_update = dict()
if peer_list == None:
peer_list = self.peer_robot_registry_list_
for robot in peer_list:
if robot not in self.peer_robot_state_client_dict_:
service_name = robot + "/get_peer_robot_state"
self.peer_robot_state_client_dict_[robot] = self.create_client(
GetPeerRobotState, service_name)
while not self.peer_robot_state_client_dict_[
robot].wait_for_service(timeout_sec=5.0):
self.get_logger().error(
'/get_peer_robot_state service not available in robot {}'.
format(robot))
req = GetPeerRobotState.Request()
service_response_future[
robot] = self.peer_robot_state_client_dict_[robot].call_async(
req)
peer_robot_state_update[robot] = False
t_0 = time.time()
peer_robot_state_done = False
while not peer_robot_state_done and time.time() - t_0 < 10.0:
peer_robot_state_done = True
for robot in peer_list:
if peer_robot_state_update[robot] == False:
# self.get_logger().info('check get_peer_robot_state response future')
if service_response_future[robot].done():
response = service_response_future[robot].result()
peer_robot_state_dict[robot] = response
peer_robot_state_update[robot] = True
else:
peer_robot_state_done = False
return peer_robot_state_dict
def getPeerRobotPosesInLocalFrameUsingTf(self):
peer_robot_pose_dict = dict()
for robot in self.peer_robot_registry_list_:
when = rclpy.time.Time()
try:
# Suspends callback until transform becomes available
t_0 = time.time()
transform = self._tf_buffer.lookup_transform(
robot + "/map",
robot + "/base_footprint",
when,
timeout=Duration(seconds=5.0))
# self.get_logger().info('Got {}'.format(repr(transform)))
curr_robot_pose_local_frame = Pose()
curr_robot_pose_local_frame.position.x = transform.transform.translation.x
curr_robot_pose_local_frame.position.y = transform.transform.translation.y
curr_robot_pose_local_frame.position.z = transform.transform.translation.z
curr_robot_pose_local_frame.orientation.x = transform.transform.rotation.x
curr_robot_pose_local_frame.orientation.y = transform.transform.rotation.y
curr_robot_pose_local_frame.orientation.z = transform.transform.rotation.z
curr_robot_pose_local_frame.orientation.w = transform.transform.rotation.w
t_1 = time.time()
#self.get_logger().info('(getRobotCurrentPos): robot pos:({},{}), used time:{}'.format(curr_robot_pose_local_frame.position.x, curr_robot_pose_local_frame.position.y, t_1 - t_0))
peer_robot_pose_dict[robot] = curr_robot_pose_local_frame
except LookupException as e:
self.get_logger().error(
'failed to get transform for robot {},{}'.format(
robot, repr(e)))
return peer_robot_pose_dict
def getPeerRobotPosesInLocalFrame(self):
service_client_dict = dict()
service_response_future = dict()
peer_robot_pose_dict = dict()
# always try to request and merge all the peers, no matter whether discovered at current timestep
# for robot in self.robot_peers_:
#self.peer_map_.clear()
#self.peer_local_frontiers_.clear()
for robot in self.peer_robot_registry_list_:
service_name = robot + '/get_current_robot_pose'
service_client_dict[robot] = self.create_client(
GetPeerRobotPose, service_name)
while not service_client_dict[robot].wait_for_service(
timeout_sec=5.0):
self.get_logger().info(
'/get_current_robot_pose service not available, waiting again...'
)
req = GetPeerRobotPose.Request()
req.robot_name.data = robot
service_response_future[robot] = service_client_dict[
robot].call_async(req)
# rclpy.spin_once(self)
# self.peer_data_updated_[robot] = False
# self.peer_map_[robot] = service_response_future[robot].map
# self.peer_local_frontiers_[robot] = service_response_future[robot].local_frontier
# self.peer_data_updated_[robot] = True
# response = service_response_future[robot].result()
t_0 = time.time()
peer_update_done = False
while not peer_update_done and time.time() - t_0 < 3:
peer_update_done = True
for robot in self.peer_robot_registry_list_:
#rclpy.spin_once(self)
self.get_logger().error('check service response future')
if service_response_future[robot].done():
response = service_response_future[robot].result()
peer_robot_pose_dict[robot] = response.robot_pose
# print(self.peer_map_)
# self.peer_data_updated_[robot] = True
else:
peer_update_done = False
return peer_robot_pose_dict
#TODO: currently only supports translation between local map frame and the world frame
def getPeerRobotPosesInWorldFrame(self):
peer_robot_pose_local_frame_dict = self.getPeerRobotPosesInLocalFrameUsingTf(
)
peer_robot_pose_world_frame_dict = dict()
for robot in self.peer_robot_registry_list_:
if robot in self.init_offset_dict_:
init_pose = self.init_offset_dict_[robot]
pose_world_frame = peer_robot_pose_local_frame_dict[robot]
pose_world_frame.position.x = pose_world_frame.position.x + init_pose.position.x
pose_world_frame.position.y = pose_world_frame.position.y + init_pose.position.y
pose_world_frame.position.z = pose_world_frame.position.z + init_pose.position.z
peer_robot_pose_world_frame_dict[robot] = pose_world_frame
else:
self.get_logger().error(
'(getPeerRobotPosesInWorldFrame) {} not in self.init_offset_dict_'
.format(robot))
return peer_robot_pose_world_frame_dict
def getPeerRobotPosesInCurrentRobotFrame(self):
peer_robot_pose_world_frame_dict = self.getPeerRobotPosesInWorldFrame()
peer_robot_pose_current_robot_frame_dict = dict()
for robot in self.peer_robot_registry_list_:
if robot == self.robot_name_:
peer_robot_pose_current_robot_frame_dict[robot] = Pose()
peer_robot_pose_current_robot_frame_dict[
robot].position.x = 0.0
peer_robot_pose_current_robot_frame_dict[
robot].position.y = 0.0
peer_robot_pose_current_robot_frame_dict[
robot].position.z = 0.0
peer_robot_pose_current_robot_frame_dict[
robot].orientation.x = 0.0
peer_robot_pose_current_robot_frame_dict[
robot].orientation.y = 0.0
peer_robot_pose_current_robot_frame_dict[
robot].orientation.z = 0.0
peer_robot_pose_current_robot_frame_dict[
robot].orientation.w = 1.0
else:
peer_robot_pose_current_robot_frame_dict[
robot] = peer_robot_pose_world_frame_dict[robot]
peer_robot_pose_current_robot_frame_dict[
robot].position.x -= peer_robot_pose_world_frame_dict[
self.robot_name_].position.x
peer_robot_pose_current_robot_frame_dict[
robot].position.y -= peer_robot_pose_world_frame_dict[
self.robot_name_].position.y
peer_robot_pose_current_robot_frame_dict[
robot].position.z -= peer_robot_pose_world_frame_dict[
self.robot_name_].position.z
#TODO rotation not dealt with
return peer_robot_pose_current_robot_frame_dict
def distBetweenRobotPoses(self, pose_a, pose_b):
x_a = pose_a.position.x
y_a = pose_a.position.y
x_b = pose_b.position.x
y_b = pose_b.position.y
dist = math.sqrt((x_a - x_b) * (x_a - x_b) + (y_a - y_b) * (y_a - y_b))
return dist
#each robot calculates the cluster it belongs to using BFS, the neighborhood is determined based on distance,
# then send its priority id among its cluster, then build consensus among the cluster, about which robot is
# the leader.
def getCluster(self):
peer_robot_pose_world_frame_dict = self.getPeerRobotPosesInWorldFrame()
#self.get_logger().info('(getCluster)after getPeerRobotPosesInWorldFrame(), size:{}'.format(len(peer_robot_pose_world_frame_dict)))
bfs_queue = deque()
bfs_queue.append(self.robot_name_)
cluster_list = []
is_visited_map = dict()
is_visited_map[self.robot_name_] = True
while len(bfs_queue) > 0:
curr_robot = bfs_queue[0]
bfs_queue.popleft()
#TODO check whether index curr_robot exists
curr_pose = peer_robot_pose_world_frame_dict[curr_robot]
cluster_list.append(curr_robot)
for neigh in peer_robot_pose_world_frame_dict:
if neigh == curr_robot or neigh in is_visited_map:
continue
neigh_pose = peer_robot_pose_world_frame_dict[neigh]
dist = self.distBetweenRobotPoses(curr_pose, neigh_pose)
if dist < self.cluster_range_limit_:
is_visited_map[neigh] = True
bfs_queue.append(neigh)
self.current_cluster_ = copy.deepcopy(cluster_list)
return cluster_list
def getClusterAndPoses(self):
cluster_pose_dict = dict()
peer_robot_pose_local_frame_dict = self.getPeerRobotPosesInCurrentRobotFrame(
)
# self.get_logger().info('(getCluster)after getPeerRobotPosesInWorldFrame(), size:{}'.format(len(peer_robot_pose_local_frame_dict)))
bfs_queue = deque()
bfs_queue.append(self.robot_name_)
cluster_list = []
is_visited_map = dict()
is_visited_map[self.robot_name_] = True
while len(bfs_queue) > 0:
curr_robot = bfs_queue[0]
bfs_queue.popleft()
#TODO check whether index curr_robot exists
curr_pose = peer_robot_pose_local_frame_dict[curr_robot]
cluster_list.append(curr_robot)
for neigh in peer_robot_pose_local_frame_dict:
if neigh == curr_robot or neigh in is_visited_map:
continue
neigh_pose = peer_robot_pose_local_frame_dict[neigh]
dist = self.distBetweenRobotPoses(curr_pose, neigh_pose)
if dist < self.cluster_range_limit_:
is_visited_map[neigh] = True
bfs_queue.append(neigh)
self.current_cluster_ = copy.deepcopy(cluster_list)
for c in cluster_list:
cluster_pose_dict[c] = peer_robot_pose_local_frame_dict[c]
return cluster_list, cluster_pose_dict
def getPeerStatePid(self, peer_list=None):
service_client_dict = dict()
service_response_future = dict()
peer_robot_state_pid_dict = dict()
peer_robot_state_update = dict()
if peer_list == None:
peer_list = self.peer_robot_registry_list_
for robot in peer_list:
if robot not in self.peer_robot_state_and_pic_client_dict_:
service_name = robot + "/get_peer_robot_state_and_pid"
self.peer_robot_state_and_pic_client_dict_[
robot] = self.create_client(GetPeerRobotStatePid,
service_name)
while not self.peer_robot_state_and_pic_client_dict_[
robot].wait_for_service(timeout_sec=5.0):
self.get_logger().error(
'/get_peer_robot_state_and_pid service not available in robot {}'
.format(robot))
req = GetPeerRobotStatePid.Request()
service_response_future[
robot] = self.peer_robot_state_and_pic_client_dict_[
robot].call_async(req)
peer_robot_state_update[robot] = False
t_0 = time.time()
peer_robot_state_pid_done = False
while not peer_robot_state_pid_done and time.time() - t_0 < 10.0:
peer_robot_state_pid_done = True
for robot in peer_list:
if peer_robot_state_update[robot] == False:
# self.get_logger().info('check get_peer_robot_state response future')
if service_response_future[robot].done():
response = service_response_future[robot].result()
peer_robot_state_pid_dict[robot] = response
peer_robot_state_update[robot] = True
else:
peer_robot_state_pid_done = False
return peer_robot_state_pid_dict
#request other robots' priority id in the same cluster, if any priority id is greater then current robot's priority id, then current robot is not leader, otherwise current robot is leader
def amILeaderAmongCluster(self, cluster_list):
service_response_future = dict()
peer_robot_pid = dict()
for robot in cluster_list:
if robot not in self.pid_request_client_dict_:
service_name = robot + "/get_peer_robot_pid"
self.pid_request_client_dict_[robot] = self.create_client(
GetPeerRobotPid, service_name)
while not self.pid_request_client_dict_[robot].wait_for_service(
timeout_sec=5.0):
self.get_logger().error(
'/get_peer_robot_pid service not available in robot {}'.
format(robot))
req = GetPeerRobotPid.Request()
service_response_future[robot] = self.pid_request_client_dict_[
robot].call_async(req)
t_0 = time.time()
peer_pid_done = False
while not peer_pid_done and time.time() - t_0 < 3.0:
peer_pid_done = True
for robot in self.current_cluster_:
rclpy.spin_once(self)
self.get_logger().info('check pid service response future')
if service_response_future[robot].done():
response = service_response_future[robot].result()
peer_robot_pid[robot] = response.pid
else:
peer_pid_done = False
for robot in peer_robot_pid:
if robot == self.robot_name_:
continue
if peer_robot_pid[robot] > self.current_robot_pid_:
return False
return True
def assignTargetForCluster(self):
pass
def requestControlPermissionForCluster(self):
pass
def debugPrint(self):
print('PeerInterfaceNode: debug')
def navigateToPose(self, target_pose):
self.navigate_to_pose_state_ = self.e_util.NAVIGATION_MOVING
target_pose_stamped = PoseStamped()
target_pose_stamped.pose = target_pose
goal_msg = NavigateToPose.Goal()
goal_msg.pose = target_pose_stamped
self.navigate_to_pose_client_.wait_for_server()
# send_goal_async test
self.send_goal_future = self.navigate_to_pose_client_.send_goal_async(
goal_msg)
self.send_goal_future.add_done_callback(
self.navigateToPoseResponseCallback)
return self.send_goal_future
def navigateToPoseResponseCallback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().info('NavigateToPoseGoal rejected')
return
self.get_logger().info('NavigateToPoseGoal accepted')
self.get_result_future = goal_handle.get_result_async()
self.get_result_future.add_done_callback(
self.navigateToPoseResultCallback)
def navigateToPoseResultCallback(self, future):
status = future.result().status
result = future.result().result
if status == GoalStatus.STATUS_SUCCEEDED:
# self.get_logger().info('Goal succeeded! Result: {0}'.format(result.sequence))
self.get_logger().info('navigateToPoseAction finished!')
self.navigate_to_pose_state_ = self.e_util.NAVIGATION_DONE
else:
self.get_logger().info(
'navigateToPoseAction failed with status: {0}'.format(status))
self.navigate_to_pose_state_ = self.e_util.NAVIGATION_FAILED
def sendTargetForPeer(self, peer, target_pose_peer_frame):
service_name = peer + '/set_robot_target_pose'
set_peer_robot_target_client = self.create_client(
SetRobotTargetPose, service_name)
while not set_peer_robot_target_client.wait_for_service(
timeout_sec=5.0):
self.get_logger().error(
'/set_robot_target_pose service not available in robot {}'.
format(peer))
req = SetRobotTargetPose.Request()
req.request_robot_name.data = self.robot_name_
req.target_pose = target_pose_peer_frame
set_peer_robot_target_future = set_peer_robot_target_client.call_async(
req)
self.get_logger().error('Send COMMAND to {} from {}'.format(
peer, self.robot_name_))
t_0 = time.time()
send_peer_target_done = False
while not send_peer_target_done and time.time() - t_0 < 10.0:
send_peer_target_done = True
if set_peer_robot_target_future.done():
response = set_peer_robot_target_future.result()
self.get_logger().error(
'got response, successfully set_robot_target_pose for robot {}'
.format(peer))
else:
send_peer_target_done = False
class TestBroadcasterAndListener(unittest.TestCase):
@classmethod
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)
@classmethod
def tearDownClass(cls):
rclpy.shutdown()
def setUp(self):
self.buffer = Buffer()
self.listener.buffer = self.buffer
def broadcast_transform(self, target_frame, source_frame, time_stamp):
broadcast_transform = build_transform(
target_frame=target_frame, source_frame=source_frame, stamp=time_stamp)
self.broadcaster.sendTransform(broadcast_transform)
self.executor.spin_once()
return broadcast_transform
def broadcast_static_transform(self, target_frame, source_frame, time_stamp):
broadcast_transform = build_transform(
target_frame=target_frame, source_frame=source_frame, stamp=time_stamp)
self.static_broadcaster.sendTransform(broadcast_transform)
self.executor.spin_once()
return broadcast_transform
def test_broadcaster_and_listener(self):
time_stamp = rclpy.time.Time(seconds=1, nanoseconds=0).to_msg()
broadcasted_transform = self.broadcast_transform(
target_frame='foo', source_frame='bar', time_stamp=time_stamp)
listened_transform = self.buffer.lookup_transform(
target_frame='foo', source_frame='bar', time=time_stamp)
self.assertTrue(broadcasted_transform, listened_transform)
# Execute a coroutine
listened_transform_async = None
coro = self.buffer.lookup_transform_async(
target_frame='foo', source_frame='bar', time=time_stamp)
try:
coro.send(None)
except StopIteration as e:
# The coroutine finished; store the result
coro.close()
listened_transform_async = e.value
self.assertTrue(broadcasted_transform, listened_transform_async)
def test_extrapolation_exception(self):
self.broadcast_transform(
target_frame='foo', source_frame='bar',
time_stamp=rclpy.time.Time(seconds=0.3, nanoseconds=0).to_msg())
self.broadcast_transform(
target_frame='foo', source_frame='bar',
time_stamp=rclpy.time.Time(seconds=0.2, nanoseconds=0).to_msg())
with self.assertRaises(ExtrapolationException) as e:
self.buffer.lookup_transform(
target_frame='foo', source_frame='bar',
time=rclpy.time.Time(seconds=0.1, nanoseconds=0).to_msg())
self.assertTrue(
'Lookup would require extrapolation into the past' in str(e.exception))
with self.assertRaises(ExtrapolationException) as e:
self.buffer.lookup_transform(
target_frame='foo', source_frame='bar',
time=rclpy.time.Time(seconds=0.4, nanoseconds=0).to_msg())
self.assertTrue(
'Lookup would require extrapolation into the future' in str(e.exception))
def test_static_broadcaster_and_listener(self):
broadcasted_transform = self.broadcast_static_transform(
target_frame='foo', source_frame='bar',
time_stamp=rclpy.time.Time(seconds=1.1, nanoseconds=0).to_msg())
listened_transform = self.buffer.lookup_transform(
target_frame='foo', source_frame='bar',
time=rclpy.time.Time(seconds=1.5, nanoseconds=0).to_msg())
self.assertTrue(broadcasted_transform, listened_transform)
class VelocityChecker(Node):
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 printInfo(self):
self.count = self.count % 30
if self.count == 0:
self.get_logger().info("")
self.get_logger().info(
"| Map Limit | Behavior | Obs Avoid | Obs Stop | External Lim | LatAcc Filtered | Optimized | Control VelCmd | Control AccCmd | Vehicle VelCmd | Vehicle AccCmd | Engage | Localization Vel | Vehicle Vel | [km/h]"
) # noqa: E501
mps2kmph = 3.6
vel_map_lim = self.data_arr[LANE_CHANGE] * mps2kmph
vel_behavior = self.data_arr[BEHAVIOR_VELOCITY] * mps2kmph
vel_obs_avoid = self.data_arr[OBSTACLE_AVOID] * mps2kmph
vel_obs_stop = self.data_arr[OBSTACLE_STOP] * mps2kmph
vel_external_lim = self.external_v_lim * mps2kmph
vel_latacc_filtered = self.data_arr[LAT_ACC] * mps2kmph
vel_optimized = self.data_arr[VELOCITY_OPTIMIZE] * mps2kmph
vel_ctrl_cmd = self.data_arr[CONTROL_CMD] * mps2kmph
acc_ctrl_cmd = self.data_arr[CONTROL_CMD_ACC]
vel_vehicle_cmd = self.data_arr[VEHICLE_CMD] * mps2kmph
acc_vehicle_cmd = self.data_arr[VEHICLE_CMD_ACC]
vel_localization = self.localization_twist.linear.x * mps2kmph
vel_vehicle = self.vehicle_twist.linear.x * mps2kmph
engage = ("None" if self.autoware_engage is None else
("True" if self.autoware_engage is True else "False"))
self.get_logger().info(
"| {0: 9.2f} | {1: 8.2f} | {2: 9.2f} | {3: 8.2f} | {4: 12.2f} | {5: 15.2f} | {6: 9.2f} | {7: 14.2f} | {8: 14.2f} | {9: 14.2f} | {10: 14.2f} | {11:>6s} | {12: 16.2f} | {13: 11.2f} |"
.format( # noqa: E501
vel_map_lim,
vel_behavior,
vel_obs_avoid,
vel_obs_stop,
vel_external_lim,
vel_latacc_filtered,
vel_optimized,
vel_ctrl_cmd,
acc_ctrl_cmd,
vel_vehicle_cmd,
acc_vehicle_cmd,
engage,
vel_localization,
vel_vehicle,
))
self.count += 1
def timerCallback(self):
# self.get_logger().info('timer called')
self.updatePose(REF_LINK, SELF_LINK)
self.pub_v_arr.publish(Float32MultiArrayStamped(data=self.data_arr))
self.printInfo()
def CallBackAwEngage(self, msg):
self.autoware_engage = msg.engage
def CallBackExternalVelLim(self, msg):
self.external_v_lim = msg.max_velocity
def CallBackLocalizationTwist(self, msg):
self.localization_twist = msg.twist
def CallBackVehicleTwist(self, msg):
self.vehicle_twist = msg.twist.twist
def CallBackBehaviorPathWLid(self, msg):
# self.get_logger().info('LANE_CHANGE called')
closest = self.calcClosestPathWLid(msg)
self.data_arr[LANE_CHANGE] = msg.points[closest].point.twist.linear.x
return
def CallBackBehaviorPath(self, msg):
# self.get_logger().info('BEHAVIOR_VELOCITY called')
closest = self.calcClosestPath(msg)
self.data_arr[BEHAVIOR_VELOCITY] = msg.points[closest].twist.linear.x
return
def CallBackAvoidTrajectory(self, msg):
# self.get_logger().info('OBSTACLE_AVOID called')
closest = self.calcClosestTrajectory(msg)
self.data_arr[OBSTACLE_AVOID] = msg.points[closest].twist.linear.x
return
def CallBackLaneDriveTrajectory(self, msg):
# self.get_logger().info('OBSTACLE_STOP called')
closest = self.calcClosestTrajectory(msg)
self.data_arr[OBSTACLE_STOP] = msg.points[closest].twist.linear.x
return
def CallBackLataccTrajectory(self, msg):
# self.get_logger().info('LAT_ACC called')
closest = self.calcClosestTrajectory(msg)
self.data_arr[LAT_ACC] = msg.points[closest].twist.linear.x
return
def CallBackScenarioTrajectory(self, msg):
# self.get_logger().info('VELOCITY_OPTIMIZE called')
closest = self.calcClosestTrajectory(msg)
self.data_arr[VELOCITY_OPTIMIZE] = msg.points[closest].twist.linear.x
return
def CallBackControlCmd(self, msg):
# self.get_logger().info('CONTROL_CMD called')
self.data_arr[CONTROL_CMD] = msg.control.velocity
self.data_arr[CONTROL_CMD_ACC] = msg.control.acceleration
return
def CallBackVehicleCmd(self, msg):
# self.get_logger().info('VEHICLE_CMD called')
self.data_arr[VEHICLE_CMD] = msg.control.velocity
self.data_arr[VEHICLE_CMD_ACC] = msg.control.acceleration
return
def calcClosestPath(self, path):
closest = -1
min_dist_squared = 1.0e10
for i in range(0, len(path.points)):
dist_sq = self.calcSquaredDist2d(self.self_pose,
path.points[i].pose)
if dist_sq < min_dist_squared:
min_dist_squared = dist_sq
closest = i
return closest
def calcClosestPathWLid(self, path):
closest = -1
min_dist_squared = 1.0e10
for i in range(0, len(path.points)):
dist_sq = self.calcSquaredDist2d(self.self_pose,
path.points[i].point.pose)
if dist_sq < min_dist_squared:
min_dist_squared = dist_sq
closest = i
return closest
def calcClosestTrajectory(self, path):
closest = -1
min_dist_squared = 1.0e10
for i in range(0, len(path.points)):
dist_sq = self.calcSquaredDist2d(self.self_pose,
path.points[i].pose)
if dist_sq < min_dist_squared:
min_dist_squared = dist_sq
closest = i
return closest
def calcSquaredDist2d(self, p1, p2):
dx = p1.position.x - p2.position.x
dy = p1.position.y - p2.position.y
return dx * dx + dy * dy
def updatePose(self, from_link, to_link):
try:
tf = self.tf_buffer.lookup_transform(from_link, to_link,
rclpy.time.Time())
self.self_pose.position.x = tf.transform.translation.x
self.self_pose.position.y = tf.transform.translation.y
self.self_pose.position.z = tf.transform.translation.z
self.self_pose.orientation.x = tf.transform.rotation.x
self.self_pose.orientation.y = tf.transform.rotation.y
self.self_pose.orientation.z = tf.transform.rotation.z
self.self_pose.orientation.w = tf.transform.rotation.w
return
except LookupException as e:
self.get_logger().warn(
"No required transformation found: `{}`".format(str(e)))
return
class InteractiveMarkerGoal(object):
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 transformPose(self, target_frame, pose, time=None):
transform = self.tfBuffer.lookup_transform(
target_frame, pose.header.frame_id,
pose.header.stamp if time is not None else rospy.Time(0),
rospy.Duration(1.0))
new_pose = do_transform_pose(pose, transform)
return new_pose
def makeBox(self, msg):
marker = Marker()
marker.type = Marker.SPHERE
marker.scale.x = msg.scale * 0.2
marker.scale.y = msg.scale * 0.2
marker.scale.z = msg.scale * 0.2
marker.color.r = 0.5
marker.color.g = 0.5
marker.color.b = 0.5
marker.color.a = 0.5
return marker
def makeBoxControl(self, msg):
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(self.makeBox(msg))
msg.controls.append(control)
return control
def make6DofMarker(self, interaction_mode, root_link, tip_link):
def normed_q(x, y, z, w):
return np.array([x, y, z, w]) / np.linalg.norm([x, y, z, w])
int_marker = InteractiveMarker()
p = PoseStamped()
p.header.frame_id = tip_link
p.pose.orientation.w = 1
int_marker.header.frame_id = tip_link
int_marker.pose.orientation.w = 1
int_marker.pose = self.transformPose(root_link, p).pose
int_marker.header.frame_id = root_link
int_marker.scale = .2
int_marker.name = "eef_{}_to_{}".format(root_link, tip_link)
# insert a box
self.makeBoxControl(int_marker)
int_marker.controls[0].interaction_mode = interaction_mode
if interaction_mode != InteractiveMarkerControl.NONE:
control_modes_dict = {
InteractiveMarkerControl.MOVE_3D: "MOVE_3D",
InteractiveMarkerControl.ROTATE_3D: "ROTATE_3D",
InteractiveMarkerControl.MOVE_ROTATE_3D: "MOVE_ROTATE_3D"
}
int_marker.name += "_" + control_modes_dict[interaction_mode]
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
return int_marker
class process_feedback(object):
def __init__(self, i_server, client, root_link, tip_link):
self.i_server = i_server
self.client = client
self.tip_link = tip_link
self.root_link = root_link
def __call__(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
print('interactive goal update')
goal = ControllerListGoal()
goal.type = ControllerListGoal.STANDARD_CONTROLLER
# asd = 'asd'
# translation
controller = Controller()
controller.type = Controller.TRANSLATION_3D
controller.tip_link = self.tip_link
controller.root_link = self.root_link
controller.goal_pose.header = feedback.header
controller.goal_pose.pose = feedback.pose
controller.p_gain = 3
controller.enable_error_threshold = True
controller.threshold_value = 0.05
controller.weight = 1.0
goal.controllers.append(controller)
# rotation
controller = Controller()
controller.type = Controller.ROTATION_3D
controller.tip_link = self.tip_link
controller.root_link = self.root_link
controller.goal_pose.header = feedback.header
controller.goal_pose.pose = feedback.pose
controller.p_gain = 3
controller.enable_error_threshold = True
controller.threshold_value = 0.2
controller.weight = 1.0
goal.controllers.append(controller)
self.client.send_goal(goal)
self.i_server.applyChanges()
class Executor(Node):
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 new_trajectory(self, msg: Path):
new_path = [
pose_to_matrix(pose_stamped.pose)
for pose_stamped in msg.poses
]
self.steps_trajectory.clear()
self.steps_trajectory.extend(new_path)
def timer_callback(self):
if self.first_time:
self.first_time = False
self.start_timer.cancel()
self.timer = self.create_timer(UPDATE_PERIOD, self.timer_callback)
start = self.get_clock().now()
diff = start.nanoseconds - self.prev_time.nanoseconds
point_i = int(SERVO_FREQ * diff / (10 ** 9))
print(diff/10**9, point_i)
if point_i >= len(self.prev_trajectory):
point_i = len(self.prev_trajectory) - 1
print(point_i)
current_point = self.prev_trajectory[point_i]
new_trajectory = self.trajectory_generator.generate_trajectory(current_point)
inversed_trajectory = [
self.inv_kin_calc.calc_point(point)
for point in new_trajectory
]
data = self.trajectory_encoder.encode_trajectory(inversed_trajectory)[1:]
computation_time = self.get_clock().now() - start
cmd_size = 3 + 18 * 2
point_k = int(SERVO_FREQ * computation_time.nanoseconds / (10 ** 9))
points_passed = min(point_i + point_k, len(self.prev_trajectory) - 1) - point_i
print(points_passed)
data = bytearray([42]) + data[points_passed * cmd_size:(points_passed + 250 * 2) * cmd_size]
spi_msg = UInt8MultiArray(data=data)
self.spi_bridge.publish(spi_msg)
self.prev_time = start
self.prev_trajectory = new_trajectory[points_passed:points_passed + 250 * 2]
front_point = self.path_proxy.first_unused()
triangle = self.inv_kin_calc.triangle(front_point, left_side=False)
self.publish_transform('odom', 'front_point', front_point)
self.publish_transform('odom', 'tri_a', triangle[0])
self.publish_transform('odom', 'tri_b', triangle[1])
self.publish_transform('odom', 'tri_c', triangle[2])
elapsed = self.get_clock().now() - start
# self.get_logger().debug(f'Elapsed {elapsed.nanoseconds / 10**9:.3f}s')
print(f'Elapsed {elapsed.nanoseconds / 10**9:.3f}s')
def init_points(self):
if self._tf_buffer.can_transform('odom', 'front_point', Time()):
transform = self._tf_buffer.lookup_transform(
'odom', 'front_point', Time())
self.front_point.position.x = transform.transform.translation.x
self.front_point.position.y = transform.transform.translation.y
self.front_point.position.z = transform.transform.translation.z
else:
self.publish_transform(
'odom', 'front_point', tf.translation_matrix((0, 0, 0)))
if self._tf_buffer.can_transform('odom', 'rear_point', Time()):
transform = self._tf_buffer.lookup_transform(
'odom', 'rear_point', Time())
self.rear_point.position.x = transform.transform.translation.x
self.rear_point.position.y = transform.transform.translation.y
self.rear_point.position.z = transform.transform.translation.z
else:
self.publish_transform(
'odom', 'rear_point', tf.translation_matrix((0, 0, 0)))
while self.pub.get_subscription_count() < 1:
pass
sleep(2)
self.publish_transform('odom', 'base_link', self.base_link_start_height)
def init_test_path(self):
self.steps_trajectory = [
tf.translation_matrix((0, 0, 0)),
]
def tf_viz_callback(self):
now = self.get_clock().now()
point_i = int(SERVO_FREQ * (now.nanoseconds - self.prev_time.nanoseconds) / (10**9)) # TODO
if point_i >= len(self.prev_trajectory):
point_i = len(self.prev_trajectory) - 1
point = self.prev_trajectory[point_i]
angles = self.inv_kin_calc.calc_point(point)
names, positions = [], []
for name, position in angles.items():
names.append(name)
positions.append(position)
msg = JointState()
msg.header.stamp = header_stamp(self.get_clock().now())
msg.name = names
msg.position = positions
self.pub.publish(msg)
# base = tf.concatenate_matrices(point.base_link_pose, self.base_link_height)
base = point.base_link_pose
self.publish_transform('odom', 'base_link', base)
self.publish_transform('odom', 'right_tri', point.right_pose)
self.publish_transform('odom', 'left_tri', point.left_pose)
class TrajectoryVisualizer(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
def test(self):
self.updatePose("map", "base_link")
def CallbackLocalizationTwist(self, cmd):
self.localization_vx = cmd.twist.twist.linear.x
def CallbackVehicleTwist(self, cmd):
self.vehicle_vx = cmd.longitudinal_velocity
def CallbackMotionVelOptTraj(self, cmd1, cmd2, cmd3, cmd4, cmd5):
print("CallbackMotionVelOptTraj called")
self.CallBackTrajExVelLim(cmd1)
self.CallBackTrajLatAccFiltered(cmd2)
self.CallBackTrajRaw(cmd3)
self.CallBackTrajTimeResampled(cmd4)
self.CallBackTrajFinal(cmd5)
def CallBackTrajExVelLim(self, cmd):
self.trajectory_external_velocity_limited = cmd
self.update_ex_vel_lim = True
def CallBackTrajLatAccFiltered(self, cmd):
self.trajectory_lateral_acc_filtered = cmd
self.update_lat_acc_fil = True
def CallBackTrajRaw(self, cmd):
self.trajectory_raw = cmd
self.update_traj_raw = True
def CallBackTrajTimeResampled(self, cmd):
self.trajectory_time_resampled = cmd
self.update_traj_resample = True
def CallBackTrajFinal(self, cmd):
self.trajectory_final = cmd
self.update_traj_final = True
def CallBackLaneDrivingTraj(self, cmd6, cmd7, cmd8, cmd9):
print("CallBackLaneDrivingTraj called")
self.CallBackLaneChangePath(cmd6)
self.CallBackBehaviorPath(cmd7)
self.CallbackObstacleAvoidTraj(cmd8)
self.CallbackObstacleStopTraj(cmd9)
def CallBackLaneChangePath(self, cmd):
self.lane_change_path = cmd
self.update_lanechange_path = True
def CallBackBehaviorPath(self, cmd):
self.behavior_path = cmd
self.update_behavior_path = True
def CallbackObstacleAvoidTraj(self, cmd):
self.obstacle_avoid_traj = cmd
self.update_traj_ob_avoid = True
def CallbackObstacleStopTraj(self, cmd):
self.obstacle_stop_traj = cmd
self.update_traj_ob_stop = True
def setPlotTrajectoryVelocity(self):
self.ax1 = plt.subplot(1, 1, 1) # row, col, index(<raw*col)
(self.im1, ) = self.ax1.plot([], [],
label="0: lane_change_path",
marker="")
(self.im2, ) = self.ax1.plot([], [],
label="1: behavior_path",
marker="",
ls="--")
(self.im3, ) = self.ax1.plot([], [],
label="2: obstacle_avoid_traj",
marker="",
ls="-.")
(self.im4, ) = self.ax1.plot([], [],
label="3: obstacle_stop_traj",
marker="",
ls="--")
(self.im5, ) = self.ax1.plot([], [],
label="4-1: opt input",
marker="",
ls="--")
(self.im6, ) = self.ax1.plot(
[], [],
label="4-2: opt external_velocity_limited",
marker="",
ls="--")
(self.im7, ) = self.ax1.plot([], [],
label="4-3: opt lat_acc_filtered",
marker="",
ls="--")
(self.im8, ) = self.ax1.plot([], [],
label="4-4: opt time_resampled",
marker="",
ls="--")
(self.im9, ) = self.ax1.plot([], [],
label="4-5: opt final",
marker="",
ls="-")
(self.im10, ) = self.ax1.plot([], [],
label="localization twist vx",
color="r",
marker="*",
ls=":",
markersize=10)
(self.im11, ) = self.ax1.plot([], [],
label="vehicle twist vx",
color="k",
marker="+",
ls=":",
markersize=10)
self.ax1.set_title("trajectory's velocity")
self.ax1.legend()
self.ax1.set_xlim([0, PLOT_MAX_ARCLENGTH])
self.ax1.set_ylabel("vel [m/s]")
return (
self.im1,
self.im2,
self.im3,
self.im4,
self.im5,
self.im6,
self.im7,
self.im8,
self.im9,
self.im10,
self.im11,
)
def plotTrajectoryVelocity(self, data):
self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME)
if self.self_pose_received is False:
print("plot start but self pose is not received")
return (
self.im1,
self.im2,
self.im3,
self.im4,
self.im5,
self.im6,
self.im7,
self.im8,
self.im9,
self.im10,
self.im11,
)
print("plot start")
# copy
lane_change_path = self.lane_change_path
behavior_path = self.behavior_path
obstacle_avoid_traj = self.obstacle_avoid_traj
obstacle_stop_traj = self.obstacle_stop_traj
trajectory_raw = self.trajectory_raw
trajectory_external_velocity_limited = self.trajectory_external_velocity_limited
trajectory_lateral_acc_filtered = self.trajectory_lateral_acc_filtered
trajectory_time_resampled = self.trajectory_time_resampled
trajectory_final = self.trajectory_final
if self.update_lanechange_path:
x = self.CalcArcLengthPathWLid(lane_change_path)
y = self.ToVelListPathWLid(lane_change_path)
self.im1.set_data(x, y)
self.update_lanechange_path = False
if len(y) != 0:
self.max_vel = max(10.0, np.max(y))
self.min_vel = np.min(y)
if self.update_behavior_path:
x = self.CalcArcLengthPath(behavior_path)
y = self.ToVelListPath(behavior_path)
self.im2.set_data(x, y)
self.update_behavior_path = False
if self.update_traj_ob_avoid:
x = self.CalcArcLength(obstacle_avoid_traj)
y = self.ToVelList(obstacle_avoid_traj)
self.im3.set_data(x, y)
self.update_traj_ob_avoid = False
if self.update_traj_ob_stop:
x = self.CalcArcLength(obstacle_stop_traj)
y = self.ToVelList(obstacle_stop_traj)
self.im4.set_data(x, y)
self.update_traj_ob_stop = False
if self.update_traj_raw:
x = self.CalcArcLength(trajectory_raw)
y = self.ToVelList(trajectory_raw)
self.im5.set_data(x, y)
self.update_traj_raw = False
if self.update_ex_vel_lim:
x = self.CalcArcLength(trajectory_external_velocity_limited)
y = self.ToVelList(trajectory_external_velocity_limited)
self.im6.set_data(x, y)
self.update_ex_vel_lim = False
if self.update_lat_acc_fil:
x = self.CalcArcLength(trajectory_lateral_acc_filtered)
y = self.ToVelList(trajectory_lateral_acc_filtered)
self.im7.set_data(x, y)
self.update_lat_acc_fil = False
if self.update_traj_resample:
x = self.CalcArcLength(trajectory_time_resampled)
y = self.ToVelList(trajectory_time_resampled)
self.im8.set_data(x, y)
self.update_traj_resample = False
if self.update_traj_final:
x = self.CalcArcLength(trajectory_final)
y = self.ToVelList(trajectory_final)
self.im9.set_data(x, y)
self.update_traj_final = False
closest = self.calcClosestTrajectory(trajectory_final)
if closest >= 0:
x_closest = x[closest]
self.im10.set_data(x_closest, self.localization_vx)
self.im11.set_data(x_closest, self.vehicle_vx)
# change y-range
self.ax1.set_ylim([self.min_vel - 1.0, self.max_vel + 1.0])
return (
self.im1,
self.im2,
self.im3,
self.im4,
self.im5,
self.im6,
self.im7,
self.im8,
self.im9,
self.im10,
self.im11,
)
def CalcArcLength(self, traj):
s_arr = []
ds = 0.0
s_sum = 0.0
if len(traj.points) > 0:
s_arr.append(s_sum)
for i in range(1, len(traj.points)):
p0 = traj.points[i - 1]
p1 = traj.points[i]
dx = p1.pose.position.x - p0.pose.position.x
dy = p1.pose.position.y - p0.pose.position.y
ds = np.sqrt(dx**2 + dy**2)
s_sum += ds
s_arr.append(s_sum)
return s_arr
def CalcArcLengthPathWLid(self, traj):
s_arr = []
ds = 0.0
s_sum = 0.0
if len(traj.points) > 0:
s_arr.append(s_sum)
for i in range(1, len(traj.points)):
p0 = traj.points[i - 1].point
p1 = traj.points[i].point
dx = p1.pose.position.x - p0.pose.position.x
dy = p1.pose.position.y - p0.pose.position.y
ds = np.sqrt(dx**2 + dy**2)
s_sum += ds
s_arr.append(s_sum)
return s_arr
def CalcArcLengthPath(self, traj):
s_arr = []
ds = 0.0
s_sum = 0.0
if len(traj.points) > 0:
s_arr.append(s_sum)
for i in range(1, len(traj.points)):
p0 = traj.points[i - 1]
p1 = traj.points[i]
dx = p1.pose.position.x - p0.pose.position.x
dy = p1.pose.position.y - p0.pose.position.y
ds = np.sqrt(dx**2 + dy**2)
s_sum += ds
s_arr.append(s_sum)
return s_arr
def ToVelList(self, traj):
v_list = []
for p in traj.points:
v_list.append(p.longitudinal_velocity_mps)
return v_list
def ToVelListPathWLid(self, traj):
v_list = []
for p in traj.points:
v_list.append(p.point.longitudinal_velocity_mps)
return v_list
def ToVelListPath(self, traj):
v_list = []
for p in traj.points:
v_list.append(p.longitudinal_velocity_mps)
return v_list
def CalcAcceleration(self, traj):
a_arr = []
for i in range(1, len(traj.points) - 1):
p0 = traj.points[i - 1]
p1 = traj.points[i]
v0 = p0.longitudinal_velocity_mps
v1 = p1.longitudinal_velocity_mps
v = 0.5 * (v1 + v0)
dx = p1.pose.position.x - p0.pose.position.x
dy = p1.pose.position.y - p0.pose.position.y
ds = np.sqrt(dx**2 + dy**2)
dt = ds / max(abs(v), 0.001)
a = (v1 - v0) / dt
a_arr.append(a)
if len(traj.points) > 0:
a_arr.append(0)
a_arr.append(0)
return a_arr
def CalcJerk(self, traj):
j_arr = []
for i in range(1, len(traj.points) - 2):
p0 = traj.points[i - 1]
p1 = traj.points[i]
p2 = traj.points[i + 1]
v0 = p0.longitudinal_velocity_mps
v1 = p1.longitudinal_velocity_mps
v2 = p2.longitudinal_velocity_mps
dx0 = p1.pose.position.x - p0.pose.position.x
dy0 = p1.pose.position.y - p0.pose.position.y
ds0 = np.sqrt(dx0**2 + dy0**2)
dx1 = p2.pose.position.x - p1.pose.position.x
dy1 = p2.pose.position.y - p1.pose.position.y
ds1 = np.sqrt(dx1**2 + dy1**2)
dt0 = ds0 / max(abs(0.5 * (v1 + v0)), 0.001)
dt1 = ds1 / max(abs(0.5 * (v2 + v1)), 0.001)
a0 = (v1 - v0) / max(dt0, 0.001)
a1 = (v2 - v1) / max(dt1, 0.001)
j = (a1 - a0) / max(dt1, 0.001)
j_arr.append(j)
if len(traj.points) > 0:
j_arr.append(0)
j_arr.append(0)
j_arr.append(0)
return j_arr
def setPlotTrajectory(self):
self.ax1 = plt.subplot(3, 1, 1) # row, col, index(<raw*col)
(self.im0, ) = self.ax1.plot([], [], label="0: raw", marker="")
(self.im1, ) = self.ax1.plot([], [],
label="3: time_resampled",
marker="")
(self.im2, ) = self.ax1.plot([], [],
label="4: final velocity",
marker="")
self.ax1.set_title("trajectory's velocity")
self.ax1.legend()
self.ax1.set_xlim([0, PLOT_MAX_ARCLENGTH])
self.ax1.set_ylabel("vel [m/s]")
self.ax2 = plt.subplot(3, 1, 2)
self.ax2.set_xlim([0, PLOT_MAX_ARCLENGTH])
self.ax2.set_ylim([-1, 1])
self.ax2.set_ylabel("acc [m/ss]")
(self.im3, ) = self.ax2.plot([], [], label="final accel")
self.ax3 = plt.subplot(3, 1, 3)
self.ax3.set_xlim([0, PLOT_MAX_ARCLENGTH])
self.ax3.set_ylim([-2, 2])
self.ax3.set_xlabel("arclength [m]")
self.ax3.set_ylabel("jerk [m/sss]")
(self.im4, ) = self.ax3.plot([], [], label="final jerk")
return self.im0, self.im1, self.im2, self.im3, self.im4
def plotTrajectory(self, data):
print("plot called")
self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME)
# copy
trajectory_raw = self.trajectory_raw
# trajectory_external_velocity_limited = self.trajectory_external_velocity_limited
# trajectory_lateral_acc_filtered = self.trajectory_lateral_acc_filtered
trajectory_time_resampled = self.trajectory_time_resampled
trajectory_final = self.trajectory_final
# ax1
if self.update_traj_raw:
x = self.CalcArcLength(trajectory_raw)
y = self.ToVelList(trajectory_raw)
self.im0.set_data(x, y)
self.update_traj_raw = False
if len(y) != 0:
self.max_vel = max(10.0, np.max(y))
self.min_vel = np.min(y)
# change y-range
self.ax1.set_ylim([self.min_vel - 1.0, self.max_vel + 1.0])
if self.update_traj_resample:
x = self.CalcArcLength(trajectory_time_resampled)
y = self.ToVelList(trajectory_time_resampled)
self.im1.set_data(x, y)
self.update_traj_resample = False
if self.update_traj_final:
x = self.CalcArcLength(trajectory_final)
y = self.ToVelList(trajectory_final)
self.im2.set_data(x, y)
self.update_traj_final = False
# ax2
y = self.CalcAcceleration(trajectory_final)
if len(y) != 0:
self.max_accel = max(0.0, np.max(y))
self.min_accel = min(0.0, np.min(y))
# change y-range
self.ax2.set_ylim([self.min_accel - 1.0, self.max_accel + 1.0])
if len(x) == len(y):
self.im3.set_data(x, y)
# ax3
y = self.CalcJerk(trajectory_final)
if len(y) != 0:
self.max_jerk = max(0.0, np.max(y))
self.min_jerk = min(0.0, np.min(y))
# change y-range
# self.ax3.set_ylim([self.min_jerk - 1.0, self.max_jerk + 1.0])
self.ax3.set_ylim([-2.0, 2.0]) # fixed range
if len(x) == len(y):
self.im4.set_data(x, y)
return self.im0, self.im1, self.im2, self.im3, self.im4
def calcClosestPath(self, path):
closest = -1
min_dist_squared = 1.0e10
for i in range(0, len(path.points)):
dist_sq = self.calcSquaredDist2d(self.self_pose,
path.points[i].pose)
if dist_sq < min_dist_squared:
min_dist_squared = dist_sq
closest = i
return closest
def calcClosestPathWLid(self, path):
closest = -1
min_dist_squared = 1.0e10
for i in range(0, len(path.points)):
dist_sq = self.calcSquaredDist2d(self.self_pose,
path.points[i].point.pose)
if dist_sq < min_dist_squared:
min_dist_squared = dist_sq
closest = i
return closest
def calcClosestTrajectory(self, path):
closest = -1
min_dist_squared = 1.0e10
for i in range(0, len(path.points)):
dist_sq = self.calcSquaredDist2d(self.self_pose,
path.points[i].pose)
if dist_sq < min_dist_squared:
min_dist_squared = dist_sq
closest = i
return closest
def calcSquaredDist2d(self, p1, p2):
dx = p1.position.x - p2.position.x
dy = p1.position.y - p2.position.y
return dx * dx + dy * dy
def updatePose(self, from_link, to_link):
try:
tf = self.tf_buffer.lookup_transform(from_link, to_link,
rclpy.time.Time())
self.self_pose.position.x = tf.transform.translation.x
self.self_pose.position.y = tf.transform.translation.y
self.self_pose.position.z = tf.transform.translation.z
self.self_pose.orientation.x = tf.transform.rotation.x
self.self_pose.orientation.y = tf.transform.rotation.y
self.self_pose.orientation.z = tf.transform.rotation.z
self.self_pose.orientation.w = tf.transform.rotation.w
print("updatePose succeeded")
self.self_pose_received = True
return
except tf2_ros.TransformException:
self.get_logger().warn(
"lookup transform failed: from {} to {}".format(
from_link, to_link))
return
def closeFigure(self):
plt.close(self.fig)
class MultiExploreNode(Node):
def __init__(self, robot_name, total_robot_num):
super().__init__('multi_explore_simple_node_' + robot_name)
self.DEBUG_ = True
self.total_robot_num_ = 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.wfd_service_client = self.create_client(WfdService, self.robot_name_ + '/wfd_service')
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),
('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
print('robot peers from param file:')
print(self.persistent_robot_peers_)
print(len(self.persistent_robot_peers_))
self.e_util = ExploreUtil()
self.map_frontier_merger_ = MapAndFrontierMerger(self.robot_name_)
self.r_interface_ = RobotControlInterface(self.robot_name_)
self.r_interface_.debugPrint()
# 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.action_result_pose_ = None
def setRobotState(self, state):
self.current_state_ = state
# self.peer_interface_.current_state_ = state
def getPeerRobotInitPose(self):
for robot in self.persistent_robot_peers_:
param_name = robot + '_init_offset'
init_offset_param = self.get_parameter(param_name)
init_offset = init_offset_param.value
if robot not in self.init_offset_dict_:
self.init_offset_dict_[robot] = Pose()
self.init_offset_dict_[robot].position.x = init_offset[0]
self.init_offset_dict_[robot].position.y = init_offset[1]
self.init_offset_dict_[robot].position.z = init_offset[2]
self.init_offset_dict_[robot].orientation.x = init_offset[3]
self.init_offset_dict_[robot].orientation.y = init_offset[4]
self.init_offset_dict_[robot].orientation.z = init_offset[5]
self.init_offset_dict_[robot].orientation.w = init_offset[6]
current_robot_offset_world_pose = self.init_offset_dict_[self.robot_name_]
for peer in self.persistent_robot_peers_:
if peer == self.robot_name_:
self.init_offset_to_current_robot_dict_[peer] = Pose()
self.init_offset_to_current_robot_dict_[peer].position.x = 0.0
self.init_offset_to_current_robot_dict_[peer].position.y = 0.0
self.init_offset_to_current_robot_dict_[peer].position.z = 0.0
self.init_offset_to_current_robot_dict_[peer].orientation.x = 0.0
self.init_offset_to_current_robot_dict_[peer].orientation.y = 0.0
self.init_offset_to_current_robot_dict_[peer].orientation.z = 0.0
self.init_offset_to_current_robot_dict_[peer].orientation.w = 1.0
else:
self.init_offset_to_current_robot_dict_[peer] = Pose()
self.init_offset_to_current_robot_dict_[peer].position.x = self.init_offset_dict_[peer].position.x - current_robot_offset_world_pose.position.x
self.init_offset_to_current_robot_dict_[peer].position.y = self.init_offset_dict_[peer].position.y - current_robot_offset_world_pose.position.y
self.init_offset_to_current_robot_dict_[peer].position.z = self.init_offset_dict_[peer].position.z - current_robot_offset_world_pose.position.z
self.init_offset_to_current_robot_dict_[peer].orientation.x = 0.0
self.init_offset_to_current_robot_dict_[peer].orientation.y = 0.0
self.init_offset_to_current_robot_dict_[peer].orientation.z = 0.0
self.init_offset_to_current_robot_dict_[peer].orientation.w = 1.0
def initRobotUtil(self):
#some init work, discover peer robots, get their relative transforms from the local fixed frame
init_success = False
t_0 = time.time()
print('total robot num:'+ str(self.total_robot_num_))
print('persistent_robot_peers:' + str(len(self.persistent_robot_peers_)))
while time.time() < t_0 + 10.0 and init_success == False:
if str(len(self.persistent_robot_peers_)) == str(self.total_robot_num_):
self.get_logger().error('get peer info, try to get transform')
init_success = True
else:
self.get_logger().info('working in single_robot mode, failed to find peer robots')
init_success = False
self.getPeerRobotInitPose()
def getMapValueOnCoordsCallback(self, request, response):
self.get_logger().warn('getMapValueOnCoordsCallback!!!!!!!!!!!!!!!!!!!!!')
query_pt_list = request.query_pt_list
for query_pt in query_pt_list:
local_query_pt = query_pt
local_query_pt.x = query_pt.x - self.init_offset_dict_[self.robot_name_].position.x
local_query_pt.y = query_pt.y - self.init_offset_dict_[self.robot_name_].position.y
local_query_pt.z = query_pt.z - self.init_offset_dict_[self.robot_name_].position.z
local_cell_x = (int)((local_query_pt.x - self.inflated_local_map_.info.origin.position.x) / self.inflated_local_map_.info.resolution)
local_cell_y = (int)((local_query_pt.y - self.inflated_local_map_.info.origin.position.y) / self.inflated_local_map_.info.resolution)
local_cell_idx = local_cell_y*self.inflated_local_map_.info.width + local_cell_x
response.pt_value_list.append(self.inflated_local_map_.data[local_cell_idx])
return response
def getLocalMapAndFrontierCompressCallback(self, request, response):
# self.get_logger().warn('{} getLocalMapAndFrontierCompressRequest'.format(self.robot_name_))
if self.inflated_local_map_ == None:
self.inflated_local_map_ = OccupancyGrid()
local_map_bytes = pickle.dumps(self.inflated_local_map_)
# self.get_logger().warn('before compressed map size:{}'.format(len(local_map_bytes)))
compressed_local_map_bytes = lzma.compress(local_map_bytes)
# self.get_logger().warn('after compressed map size:{}'.format(len(compressed_local_map_bytes)))
# print(compressed_local_map_bytes)
# response.map_compress = []
# for i in range(len(compressed_local_map_bytes)):
# response.map_compress.append(compressed_local_map_bytes[i])
response.map_compress = list(compressed_local_map_bytes)
# response.map_compress = [b'a',b'c',b'r',b'w']
response.local_frontier = self.local_frontiers_msg_
# self.get_logger().warn('send map time: {}'.format(time.time()))
return response
def getLocalMapAndFrontierCallback(self, request, response):
self.get_logger().warn('{} getLocalMapAndFrontierRequest'.format(self.robot_name_))
if self.inflated_local_map_ == None:
self.inflated_local_map_ = OccupancyGrid()
response.map = self.inflated_local_map_
response.local_frontier = self.local_frontiers_msg_
self.get_logger().warn('send map time: {}'.format(time.time()))
return response
def getRobotCurrentPos(self):
#return True, if success,
#return False, if exception
when = rclpy.time.Time()
try:
# Suspends callback until transform becomes available
# t_0 = time.time()
transform = self._tf_buffer.lookup_transform(self.world_frame_, self.local_fixed_frame_,when,timeout=Duration(seconds=5.0))
# self.get_logger().info('Got {}'.format(repr(transform)))
self.current_pos_ = (transform.transform.translation.x, transform.transform.translation.y)
self.current_pose_local_frame_.pose.position.x = self.current_pos_[0]
self.current_pose_local_frame_.pose.position.y = self.current_pos_[1]
self.current_pose_local_frame_.pose.position.z = 0.0
self.current_pose_local_frame_.pose.orientation.x = transform.transform.rotation.x
self.current_pose_local_frame_.pose.orientation.y = transform.transform.rotation.y
self.current_pose_local_frame_.pose.orientation.z = transform.transform.rotation.z
self.current_pose_local_frame_.pose.orientation.w = transform.transform.rotation.w
# t_1 = time.time()
# self.get_logger().info('(getRobotCurrentPos): robot pos:({},{}), used time:{}'.format(self.current_pos_[0], self.current_pos_[1], t_1 - t_0))
return True
except LookupException as e:
self.get_logger().error('failed to get transform {}'.format(repr(e)))
return False
def localMapCallback(self, map_msg):
#do a window_WFD from robot's current position, get a set of new frontiers, and integrate the new found frontiers with existing self.local_frontiers_
#self.get_logger().warn('{}:localMapCallback'.format(self.tic_))
mutex = Lock()
mutex.acquire()
if self.local_map_callback_lock_ == True:
return
self.local_map_callback_lock_ = True
self.local_map_ = map_msg
#self.get_logger().warn('{}:before inflateMap'.format(self.tic_))
self.inflated_local_map_ = self.e_util.inflateMap(self.local_map_, 4)
#self.get_logger().warn('{}:after inflateMap'.format(self.tic_))
self.inflated_map_pub_.publish(self.local_map_)
self.local_map_callback_lock_ = False
self.tic_ = self.tic_ + 1
mutex.release()
#return
# -1 : no inflated_local_map
# -2 : can not get robot current pose from tf
# temp_window_frontiers_ , if empty, then no window_frontiers detected
def getWindowFrontiers(self):
mutex = Lock()
mutex.acquire()
temp_window_frontiers_ = []
temp_window_frontiers_msg_ = []
if self.inflated_local_map_ == None or self.inflated_local_map_ == OccupancyGrid():
self.get_logger().error('(getWindowFrontiers): no inflated_local_map')
return -1
self.get_logger().info('(getWindowFrontiers): init')
if self.getRobotCurrentPos():
# self.get_logger().warn('(updateLocalFrontiersUsingWindowWFD): start window_wfd!!!')
current_map = OccupancyGrid()
current_map.header = self.inflated_local_map_.header
current_map.info = self.inflated_local_map_.info
current_map.data = list(self.inflated_local_map_.data)
window_wfd = WindowWFD(current_map, self.current_pos_, 150)
# if self.DEBUG_ == True:
# self.inflated_map_pub_.publish(current_map)
t_0 = time.time()
window_frontiers_cell, covered_set = window_wfd.getWindowFrontiers()
t_1 = time.time()
# transform local_frontiers from the cell format to absolute position in 'double', because the cell could change due to map expansion
self.get_logger().info('(getWindowFrontiers)window_frontiers_cell size:{}, used time:{}'.format(len(window_frontiers_cell), t_1 - t_0))
for f_connect_cell in window_frontiers_cell:
f_connect = []
f_msg = Frontier()
for f_cell in f_connect_cell:
f_double = (f_cell[0]*current_map.info.resolution + current_map.info.origin.position.x, f_cell[1]*current_map.info.resolution + current_map.info.origin.position.y)
f_connect.append(f_double)
pt_stamped = PointStamped()
pt_stamped.header = current_map.header
pt_stamped.point.x = f_double[0]
pt_stamped.point.y = f_double[1]
pt_stamped.point.z = 0.0
f_msg.frontier.append(pt_stamped)
temp_window_frontiers_msg_.append(f_msg)
temp_window_frontiers_.append(f_connect)
else:
self.get_logger().error('(getWindowFrontiers): failed to get robot current pos')
return -2
mutex.release()
return temp_window_frontiers_
#detect window_WFD and integrate with local frontiers, will update self.local_frontiers_
def updateLocalFrontiersUsingWindowWFD(self):
mutex = Lock()
mutex.acquire()
temp_window_frontiers_ = []
temp_window_frontiers_msg_ = []
temp_window_frontiers_rank_ = []
if self.inflated_local_map_ == None or self.inflated_local_map_ == OccupancyGrid():
self.get_logger().error('(updateLocalFrontiersUsingWindowWFD): no inflated_local_map')
return -1, -1, -1
# self.get_logger().info('(updateLocalFrontiersUsingWindowWFD): init')
if self.getRobotCurrentPos():
# self.get_logger().warn('(updateLocalFrontiersUsingWindowWFD): start window_wfd!!!')
current_map = OccupancyGrid()
current_map.header = self.inflated_local_map_.header
current_map.info = self.inflated_local_map_.info
current_map.data = list(self.inflated_local_map_.data)
window_wfd = WindowWFD(current_map, self.current_pos_, 250)
# if self.DEBUG_ == True:
# self.inflated_map_pub_.publish(current_map)
try:
t_0 = time.time()
except:
print("unexcepted error:", sys.exc_info()[0])
window_frontiers_cell, covered_set = window_wfd.getWindowFrontiers()
t_1 = time.time()
# transform local_frontiers from the cell format to absolute position in 'double', because the cell could change due to map expansion
self.get_logger().info('(updateLocalFrontiersUsingWindowWFD)window_frontiers_cell size:{}, used time:{}'.format(len(window_frontiers_cell), t_1 - t_0))
for f_connect_cell in window_frontiers_cell:
f_connect = []
f_msg = Frontier()
if len(f_connect_cell)<6:
continue
for f_cell in f_connect_cell:
f_double = (f_cell[0]*current_map.info.resolution + current_map.info.origin.position.x, f_cell[1]*current_map.info.resolution + current_map.info.origin.position.y)
f_connect.append(f_double)
pt_stamped = PointStamped()
pt_stamped.header = current_map.header
pt_stamped.point.x = f_double[0]
pt_stamped.point.y = f_double[1]
pt_stamped.point.z = 0.0
f_msg.frontier.append(pt_stamped)
temp_window_frontiers_msg_.append(f_msg)
temp_window_frontiers_.append(f_connect)
temp_window_frontiers_rank_.append(f_connect_cell[0][2])
#update self.local_frontiers_msg and self.local_frontiers_ with temp_window_frontiers_ , temp_window_frontiers_msg_
self.updateLocalFrontiers(temp_window_frontiers_, temp_window_frontiers_msg_, window_wfd.window_size_, self.current_pos_, current_map, covered_set)
#DEBUG
if self.DEBUG_:
# self.get_logger().warn('debug local_frontiers')
local_map_dw = current_map.info.width
local_map_dh = current_map.info.height
frontiers_index_list = []
# for f_connect in window_frontiers_cell:
# for f_cell in f_connect:
# frontiers_index_list.append(f_cell[1]*local_map_dw + f_cell[0])
for f_connect in self.local_frontiers_:
for f_double in f_connect:
f_cell = ((int)((f_double[0] - current_map.info.origin.position.x) / current_map.info.resolution) , (int)((f_double[1] - current_map.info.origin.position.y) / current_map.info.resolution))
frontiers_index_list.append(f_cell[1]*local_map_dw + f_cell[0])
frontier_debug_map = OccupancyGrid()
frontier_debug_map.header = current_map.header
frontier_debug_map.info = current_map.info
temp_array = np.zeros((1, local_map_dw*local_map_dh), dtype=np.int8)
temp_array[:]=(int)(-1)
frontier_debug_map.data = temp_array.ravel().tolist()
for idx in frontiers_index_list:
if int(idx) < 0 or int(idx) > len(frontier_debug_map.data)-1:
continue
frontier_debug_map.data[int(idx)] = 0
frontier_map_width = frontier_debug_map.info.width
frontier_map_height = frontier_debug_map.info.height
self.debug_frontier_pub_.publish(frontier_debug_map)
pass
# self.get_logger().warn('end debug local_frontiers')
#DEBUG
else:
self.get_logger().error('(updateLocalFrontiersUsingWindowWFD): failed to get robot current pos')
return -2, -2, -2
self.get_logger().info('(updateLocalFrontiersUsingWindowWFD): end')
mutex.release()
return temp_window_frontiers_, temp_window_frontiers_msg_, temp_window_frontiers_rank_
def generateFrontierDebugMap(self, frontier_msg_list, current_map):
local_map_dw = current_map.info.width
local_map_dh = current_map.info.height
frontiers_index_list_w = []
frontiers_index_list_h = []
# for f_connect in local_frontiers_cell:
# for f_cell in f_connect:
# frontiers_index_list.append(f_cell[1]*local_map_dw + f_cell[0])
for f_connect_msg in frontier_msg_list:
for f_pt in f_connect_msg.frontier:
f_cell = ((int)((f_pt.point.x - current_map.info.origin.position.x) / current_map.info.resolution) , (int)((f_pt.point.y - current_map.info.origin.position.y) / current_map.info.resolution))
frontiers_index_list_h.append(f_cell[1])
frontiers_index_list_w.append(f_cell[0])
frontier_debug_map = OccupancyGrid()
frontier_debug_map.header = copy.deepcopy(current_map.header)
frontier_debug_map.info = copy.deepcopy(current_map.info)
temp_array = np.asarray(current_map.data, dtype=np.int8).reshape(local_map_dh, local_map_dw)
temp_array[:]=(int)(-1)
temp_array[frontiers_index_list_h, frontiers_index_list_w] = 0
# temp_array = np.zeros((1, local_map_dw*local_map_dh), dtype=np.int8)
frontier_debug_map.data = temp_array.ravel().tolist()
# for idx in frontiers_index_list:
# if int(idx) < 0 or int(idx) > len(frontier_debug_map.data)-1:
# continue
# frontier_debug_map.data[int(idx)] = 0
# frontier_map_width = frontier_debug_map.info.width
# frontier_map_height = frontier_debug_map.info.height
return frontier_debug_map
def updateLocalFrontiers(self, new_frontiers, new_frontiers_msg, window_size, current_pos, map, covered_set):
#update self.local_frontiers_ , self.local_frontiers_msg_ , they could be empty or already have some frontiers
if len(self.local_frontiers_) == 0:
self.local_frontiers_ = new_frontiers
self.local_frontiers_msg_ = new_frontiers_msg
else:
# print('self.local_frontiers_ size:{}'.format(len(self.local_frontiers_)))
# print('self.local_frontiers_msg_ size:{}'.format(len(self.local_frontiers_msg_)))
copied_local_frontiers_ = list(self.local_frontiers_)
copied_local_frontiers_msg_ = list(self.local_frontiers_msg_)
for old_index, old_f in enumerate(copied_local_frontiers_):
if self.e_util.isFrontierWithinWindow(old_f, current_pos, window_size * map.info.resolution - 0.5, map, covered_set) or self.e_util.isFrontierWithinObs(old_f, current_pos, window_size * map.info.resolution - 0.5, map, covered_set):
self.local_frontiers_.remove(old_f)
delete_f_msg = copied_local_frontiers_msg_[old_index]
self.local_frontiers_msg_.remove(delete_f_msg)
for new_f in new_frontiers:
self.local_frontiers_.append(new_f)
for new_f_msg in new_frontiers_msg:
self.local_frontiers_msg_.append(new_f_msg)
def updateMultiHierarchicalCoordination(self):
#state of current robot: GOING_TO_TARGET, WAITING_NEW_TARGET, REACH_TARGET, FINISH_TARGET_WINDOW_NOT_DONE,
if self.current_state_ == self.e_util.SYSTEM_INIT:
#robot rotate inplace
self.r_interface_.rotateNCircles(1, 0.4)
# self.updateWindowWFD()
self.r_interface_.stopAtPlace()
#self.current_state_ = self.TEST_MERGE_MAP
self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
elif self.current_state_ == self.e_util.GOING_TO_TARGET:
self.get_logger().error('updateMulti: enter GOING_TO_TARGET')
if self.current_target_pos_ == None:
self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
return
self.get_logger().warn('go to target:({},{}), orientation({},{},{},{})'.format(self.current_target_pos_.position.x, self.current_target_pos_.position.y, self.current_target_pos_.orientation.x, self.current_target_pos_.orientation.y, self.current_target_pos_.orientation.z, self.current_target_pos_.orientation.w))
self.r_interface_.navigateToPoseFunction(self.current_target_pos_)
# self.getRobotCurrentPos()
# direct_length_square = (self.current_pos_[0] - self.current_target_pos_.position.x)*(self.current_pos_[0] - self.current_target_pos_.position.x) + (self.current_pos_[1] - self.current_target_pos_.position.y)*(self.current_pos_[1] - self.current_target_pos_.position.y)
is_thread_started = False
check_environment_thread = Thread(target=self.checkEnvironmentFunction)
while self.r_interface_.navigate_to_pose_state_ == self.e_util.NAVIGATION_MOVING:
# self.get_logger().error('start checking environment...')
# self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
self.getRobotCurrentPos()
current_direct_length_square = (self.current_pos_[0] - self.current_target_pos_.position.x)*(self.current_pos_[0] - self.current_target_pos_.position.x) + (self.current_pos_[1] - self.current_target_pos_.position.y)*(self.current_pos_[1] - self.current_target_pos_.position.y)
# self.get_logger().warn("navigating to target {},{}".format(self.current_target_pos_.position.x, self.current_target_pos_.position.y ))
# current_direct_length_square = 10.0
if current_direct_length_square < 2.0*2.0:
if is_thread_started == False:
check_environment_thread.start()
is_thread_started = True
# return
pass
if self.r_interface_.navigate_to_pose_state_ == self.e_util.NAVIGATION_DONE:
self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
elif self.r_interface_.navigate_to_pose_state_ == self.e_util.NAVIGATION_FAILED:
self.get_logger().error('NAVIGATION_FAILED')
current_target_cell = ((int)((self.current_target_pos_.position.x - self.inflated_local_map_.info.origin.position.x) / self.inflated_local_map_.info.resolution) , (int)((self.current_target_pos_.position.y - self.inflated_local_map_.info.origin.position.y) / self.inflated_local_map_.info.resolution))
# self.getRobotCurrentPos()
# current_cell = ((int)((self.current_pos_[0] - self.inflated_local_map_.info.origin.position.x) / self.inflated_local_map_.info.resolution) , (int)((self.current_pos_[1] - self.inflated_local_map_.info.origin.position.y) / self.inflated_local_map_.info.resolution))
updated_cell = self.e_util.getFreeNeighborRandom(current_target_cell, self.inflated_local_map_, 30, 50)
if updated_cell == None:
self.get_logger().warn('updated_cell is None, CHECK_ENVIRONMENT now!!!!!')
self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
return
updated_target_pt = (updated_cell[0]*self.inflated_local_map_.info.resolution + self.inflated_local_map_.info.origin.position.x, updated_cell[1]*self.inflated_local_map_.info.resolution + self.inflated_local_map_.info.origin.position.y)
self.current_target_pos_.position.x = updated_target_pt[0]
self.current_target_pos_.position.y = updated_target_pt[1]
if self.previous_state_ != self.e_util.GOING_TO_TARGET:
self.get_logger().error('going_to_target_failed_times_ = 000000000000')
self.going_to_target_failed_times_ = 0
else:
self.going_to_target_failed_times_ += 1
self.get_logger().error('going_to_target_failed_times_ = {}{}{}{}{}'.format(self.going_to_target_failed_times_,self.going_to_target_failed_times_,self.going_to_target_failed_times_,self.going_to_target_failed_times_,self.going_to_target_failed_times_))
self.previous_state_ = self.current_state_
if self.going_to_target_failed_times_ > 10:
self.get_logger().warn('going_to_target_failed_times_ > 10')
self.get_logger().warn('going_to_target_failed_times_ > 10')
self.get_logger().warn('going_to_target_failed_times_ > 10')
self.get_logger().warn('going_to_target_failed_times_ > 10')
self.get_logger().warn('going_to_target_failed_times_ > 10')
self.last_failed_frontier_pt_ = self.current_target_pos_
self.going_to_target_failed_times_ = 0
self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
else:
self.get_logger().error('retry same target again!!!!!!!!!!!!!!!!')
self.setRobotState(self.e_util.GOING_TO_TARGET)
if is_thread_started == True:
check_environment_thread.join()
self.current_target_pos_ = self.next_target_pos_
is_thread_started == False
self.setRobotState(self.e_util.GOING_TO_TARGET)
# self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
elif self.current_state_ == self.e_util.CHECK_ENVIRONMENT:
self.get_logger().error('Enter CHECK_ENVIRONMENT')
#check current window_frontiers, if window_frontiers is empty, then FINISH_TARGET_WINDOW_DONE
#if window_frontiers is not empty, then FINISH_TARGET_WINDOW_NOT_DONE
wfd_response = self.send_wfd_service_request()
# self.send_wfd_action_goal()
# while self.is_wfd_action_finished_ != True:
# pass
self.local_frontiers_msg_ = wfd_response.local_frontiers
self.window_frontiers_msg = wfd_response.window_frontiers
self.window_frontiers_rank = wfd_response.window_frontiers_rank
# self.window_frontiers, self.window_frontiers_msg, self.window_frontiers_rank = self.updateLocalFrontiersUsingWindowWFD()
# if self.window_frontiers == -1 or self.window_frontiers == -2:
# self.get_logger().error('(update.CHECK_ENVIRONMENT) failed getting WindowFrontiers, check robot, something is wrong')
# self.current_state_ = self.e_util.CHECK_ENVIRONMENT
# else:
self.send_group_coordinator_goal()
while self.is_action_finished_ != True:
pass
self.current_target_pos_ = self.action_result_pose_
# self.group_coordinator_.setPeerInfo(self.persistent_robot_peers_, peer_pose_dict, cluster_list, cluster_pose_dict, self.window_frontiers, self.window_frontiers_rank, self.local_frontiers_, self.local_frontiers_msg_, self.inflated_local_map_, self.peer_interface_.init_offset_dict_, self.last_failed_frontier_pt_)
# self.current_target_pos_ = self.group_coordinator_.hierarchicalCoordinationAssignment()
if self.current_target_pos_ == None:
self.get_logger().error('finish local_frontiers, done for current robot')
self.setRobotState(self.e_util.FINISH_TASK)
else:
self.previous_state_ = self.e_util.CHECK_ENVIRONMENT
self.setRobotState(self.e_util.GOING_TO_TARGET)
# no window frontiers available, find target frontier from self.local_frontiers_msg_, considering distance from peer robot_tracks
elif self.current_state_ == self.e_util.FINISH_TASK:
self.r_interface_.rotateNCircles(1, 0.4)
self.get_logger().error('finish local_frontiers, done for current robot')
def send_group_coordinator_goal(self):
self._action_client.wait_for_server()
while self.is_action_finished_ == False:
pass
self.is_action_finished_ = False
goal_msg = GroupCoordinator.Goal()
for peer in self.persistent_robot_peers_:
peer_msg = String()
if peer != self.robot_name_:
peer_msg.data = peer
goal_msg.peer_list.append(peer_msg)
if self.getRobotCurrentPos():
goal_msg.robot_pose_local_frame = self.current_pose_local_frame_
else:
self.get_logger().error('(send_group_coordinator_goal) fail to get robot current pose')
return
goal_msg.window_frontiers = self.window_frontiers_msg
goal_msg.window_frontiers_rank = self.window_frontiers_rank
goal_msg.local_frontiers = self.local_frontiers_msg_
goal_msg.local_inflated_map = self.inflated_local_map_
goal_msg.last_failed_frontier_pt.pose = self.last_failed_frontier_pt_
# goal_uuid = 0
# if self.robot_name_ == 'tb0':
# goal_uuid = 0
# elif self.robot_name_ == 'tb1':
# goal_uuid = 1
self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_group_coordinator_callback)
self._send_goal_future.add_done_callback(self.goal_response_callback)
def feedback_group_coordinator_callback(self, feedback):
pass
def goal_response_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().error('group coordinator goal rejected')
return
self.get_logger().warn('group coordinator goal accepted')
self._get_result_future = goal_handle.get_result_async()
self._get_result_future.add_done_callback(self.get_result_callback)
def get_result_callback(self, future):
result = future.result().result
status = future.result().status
if status == GoalStatus.STATUS_SUCCEEDED:
self.get_logger().info('Goal succeeded!')
self.action_result_pose_ = result.current_target_pose
self.is_action_finished_ = True
else:
self.get_logger().error('Goal failed with status: {}'.format(status))
def send_wfd_service_request(self):
while not self.wfd_service_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('wfd servie not available,wait')
req = WfdService.Request()
if self.getRobotCurrentPos():
req.robot_pose_local_frame = self.current_pose_local_frame_.pose
req.local_frontiers = self.local_frontiers_msg_
req.local_inflated_map = self.inflated_local_map_
return self.wfd_service_client.call(req)
def send_wfd_action_goal(self):
self.wfd_action_client.wait_for_server()
while self.is_wfd_action_finished_ == False:
pass
self.is_wfd_action_finished_ = False
goal_msg = WfdAction.Goal()
goal_msg.local_frontiers = self.local_frontiers_msg_
if self.getRobotCurrentPos():
goal_msg.robot_pose_local_frame = self.current_pose_local_frame_.pose
else:
self.get_logger().error('(send_wfd_goal) fail to get robot current pose')
return
goal_msg.local_inflated_map = self.inflated_local_map_
self._send_wfd_goal_future = self.wfd_action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_wfd_action_callback)
self._send_wfd_goal_future.add_done_callback(self.wfd_goal_response_callback)
def feedback_wfd_action_callback(self, feedback):
pass
def wfd_goal_response_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().error('wfd goal rejected')
return
self.get_logger().warn('wfd goal accepted')
self._get_wfd_result_future = goal_handle.get_result_async()
self._get_wfd_result_future.add_done_callback(self.get_wfd_result_callback)
def get_wfd_result_callback(self, future):
result = future.result().result
status = future.result().status
if status == GoalStatus.STATUS_SUCCEEDED:
self.get_logger().info('Goal succeeded!')
self.local_frontiers_msg_ = result.local_frontiers
self.window_frontiers_msg = result.window_frontiers
self.window_frontiers_rank = result.window_frontiers_rank
# self.window_frontiers_.clear()
# for wf_msg in self.window_frontiers_msg:
self.is_wfd_action_finished_ = True
else:
self.get_logger().error('Goal failed with status: {}'.format(status))
def checkEnvironmentFunction(self):
# self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
# self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
# self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
self.get_logger().error('Enter CHECK_ENVIRONMENT_THREAD')
# self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
# self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
#check current window_frontiers, if window_frontiers is empty, then FINISH_TARGET_WINDOW_DONE
#if window_frontiers is not empty, then FINISH_TARGET_WINDOW_NOT_DONE
# self.send_wfd_action_goal()
# while self.is_wfd_action_finished_ != True:
# pass
wfd_response = self.send_wfd_service_request()
# self.send_wfd_action_goal()
# while self.is_wfd_action_finished_ != True:
# pass
self.local_frontiers_msg_ = wfd_response.local_frontiers
self.window_frontiers_msg = wfd_response.window_frontiers
self.window_frontiers_rank = wfd_response.window_frontiers_rank
# self.window_frontiers, self.window_frontiers_msg, self.window_frontiers_rank = self.updateLocalFrontiersUsingWindowWFD()
# if self.window_frontiers == -1 or self.window_frontiers == -2:
# self.get_logger().error('(update.CHECK_ENVIRONMENT) failed getting WindowFrontiers, check robot, something is wrong')
# #self.current_state_ = self.e_util.CHECK_ENVIRONMENT
# else:
self.send_group_coordinator_goal()
while self.is_action_finished_ != True:
pass
self.next_target_pos_ = self.action_result_pose_
# if self.current_target_pos_ == None:
# self.get_logger().error('finish local_frontiers, done for current robot')
# self.setRobotState(self.e_util.FINISH_TASK)
# else:
# self.previous_state_ = self.e_util.CHECK_ENVIRONMENT
# self.setRobotState(self.e_util.GOING_TO_TARGET)
# self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
# self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
self.get_logger().error('EXIT CHECK_ENVIRONMENT_THREAD, next target:{},{}'.format(self.next_target_pos_.position.x, self.next_target_pos_.position.y))
class AutoMap(Node):
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")
@staticmethod
def quaternion_to_euler(quaternion):
t0 = +2.0 * (quaternion.w * quaternion.x + quaternion.y * quaternion.z)
t1 = +1.0 - 2.0 * (quaternion.x * quaternion.x +
quaternion.y * quaternion.y)
roll = atan2(t0, t1)
t2 = +2.0 * (quaternion.w * quaternion.y - quaternion.z * quaternion.x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
pitch = asin(t2)
t3 = +2.0 * (quaternion.w * quaternion.z + quaternion.x * quaternion.y)
t4 = +1.0 - 2.0 * (quaternion.y * quaternion.y +
quaternion.z * quaternion.z)
yaw = atan2(t3, t4)
return [yaw, pitch, roll]
@staticmethod
def euler_to_quaternion(roll, pitch, yaw):
qx = sin(roll / 2) * cos(pitch / 2) * cos(yaw / 2) - cos(
roll / 2) * sin(pitch / 2) * sin(yaw / 2)
qy = cos(roll / 2) * sin(pitch / 2) * cos(yaw / 2) + sin(
roll / 2) * cos(pitch / 2) * sin(yaw / 2)
qz = cos(roll / 2) * cos(pitch / 2) * sin(yaw / 2) - sin(
roll / 2) * sin(pitch / 2) * cos(yaw / 2)
qw = cos(roll / 2) * cos(pitch / 2) * cos(yaw / 2) + sin(
roll / 2) * sin(pitch / 2) * sin(yaw / 2)
return [qx, qy, qz, qw]
def get_robot_pose(self):
try:
tf = self.tf_buffer.lookup_transform("base_link", "base_link",
Time())
self.robot_pose.x = tf.transform.translation.x
self.robot_pose.y = tf.transform.translation.y
robot_rot = tf.transform.rotation
self.robot_pose.z = self.quaternion_to_euler(robot_rot)[2]
except LookupException:
self.get_logger().info('transform not ready')
def request_map(self):
self.get_logger().info("Requesting GetMap data")
self.call = self.map_srv.call_async(GetMap.Request())
self.call.add_done_callback(self.get_map)
def get_map(self, response):
try:
# response: GetMap.Response = self.call.result()
self.get_logger().info("GetMap data received")
response_map = response.result().map
self.width = response_map.info.width
self.height = response_map.info.height
self.resolution = response_map.info.resolution
origin_yaw = self.quaternion_to_euler(
response_map.info.origin.orientation)[2]
origin_x = response_map.info.origin.position.x
origin_y = response_map.info.origin.position.y
self.map_origin = Point(x=origin_x, y=origin_y, z=origin_yaw)
self.tf_timer = self.create_timer(1.0, self.get_robot_pose)
self.map_data = list(response_map.data)
for i in range(self.height):
for j in range(self.width):
self.map_data[i * self.width + j] = response_map.data[
(self.height - 1 - i) * self.width + j]
self.pose_to_pix()
self.check_directions()
if not self.find_next_goal():
self.get_logger().info("No new goal found")
return
self.pix_to_pose()
self.send_target()
except Exception as e:
self.get_logger().info('Service call failed %r' % (e, ))
def pose_to_pix(self):
self.robot_pose_img.x = float(
round((self.map_origin.y - self.robot_pose.y) / self.resolution +
self.height))
self.robot_pose_img.y = float(
round((self.robot_pose.x - self.map_origin.x) / self.resolution))
self.robot_pose_img.z = float(
round(self.robot_pose.z - self.map_origin.z))
def pix_to_pose(self):
self.target.x = self.robot_pose_img.y * self.resolution + self.map_origin.x
self.target.y = -(
(self.robot_pose_img.x - self.height) * self.resolution -
self.map_origin.y)
self.target.z = self.robot_pose_img.z + self.map_origin.z
def is_accessible(self, x, y, safe_dist=4):
if self.map_data[int(x * self.width + y)] == 0:
for m in range(-safe_dist, safe_dist + 1):
for n in range(-safe_dist, safe_dist + 1):
if self.map_data[int((x + m) * self.width +
(y + n))] == 100:
return False
return True
return False
def is_free(self, x, y):
if self.map_data[int(x * self.width + y)] == -2:
for k in range(-1, 2):
for l in range(-1, 2): # On regarde les cellules adjacentes
if k != 0 or l != 0:
if self.map_data[int(
(x + k) * self.width + (y + l)
)] == -1: # Si une de ces cellules adjacentes est inconnue
return True
return False
else:
return False
def check_directions(self):
pile = []
for i in range(-3, 4):
for j in range(-3, 4):
pile.append([
int(self.robot_pose_img.x) + i,
int(self.robot_pose_img.y) + j
])
while pile:
[x, y] = pile.pop()
self.map_data[int(x * self.width + y)] = -2
for k in range(-1, 2):
for l in range(-1, 2):
if k != 0 or l != 0:
if self.is_accessible(x + k, y + l):
pile.append([x + k, y + l])
def find_next_goal(self, rayon=10):
while 2 * rayon < max(self.height, self.width):
for i in range(-rayon, rayon + 1):
if i == -rayon or i == rayon:
for j in range(-rayon, rayon + 1):
if self.robot_pose_img.x + i < self.width and self.robot_pose_img.y + j < self.height:
if self.is_free(self.robot_pose_img.x + i,
self.robot_pose_img.y + j):
self.target.x = self.robot_pose_img.x + i
self.target.y = self.robot_pose_img.y + j
return True
else:
if self.robot_pose_img.x + i < self.width and self.robot_pose_img.y + j < self.height:
if self.is_free(self.robot_pose_img.x + i,
self.robot_pose_img.y + rayon):
self.target.x = self.robot_pose_img.x + i
self.target.y = self.robot_pose_img.y + rayon
return True
if self.is_free(self.robot_pose_img.x + i,
self.robot_pose_img.y - rayon):
self.target.x = self.robot_pose_img.x + i
self.target.y = self.robot_pose_img.y - rayon
return True
rayon = rayon + 1
return False
def goal_reached(self, resp):
goal_handle = resp.result()
if not goal_handle.accepted:
self.get_logger().info('Goal rejected :(')
return
self.request_map()
def send_target(self):
quaternion = self.euler_to_quaternion(0, 0, self.target.z)
goal = NavigateToPose.Goal()
self.goal_nbr = self.goal_nbr + 1
goal.pose.header.seq = self.goal_nbr
goal.pose.header.stamp = Time()
goal.pose.header.frame_id = "sendGoal"
goal.pose.pose.position = Point(x=self.target.x,
y=self.target.y,
yaw=0)
goal.pose.pose.orientation.x = quaternion[0]
goal.pose.pose.orientation.y = quaternion[1]
goal.pose.pose.orientation.z = quaternion[2]
goal.pose.pose.orientation.w = quaternion[3]
self.goal = self.client.send_goal_async(goal)
self.goal.add_done_callback(self.goal_reached)
class WIGGLE(object):
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 giskard_status_cb(self, data):
if len(data.status_list) > 0:
self.is_giskard_active = data.status_list[
-1].status == GoalStatus.ACTIVE
def change_cycle_time(self, new_cycle_time):
self.rate = int(self.steps / new_cycle_time)
def transformPose(self, target_frame, pose):
transform = self.tfBuffer.lookup_transform(
target_frame,
pose.header.frame_id, # source frame
rospy.Time(0), # get the tf at first available time
rospy.Duration(1.0))
new_pose = do_transform_pose(pose, transform)
return new_pose
def wiggle_cb(self, goal):
# check for invalid goals
if goal.arm != WiggleGoal.LEFT_ARM and goal.arm != WiggleGoal.RIGHT_ARM:
rospy.logerr('arm goal should be {} or {}'.format(
WiggleGoal.LEFT_ARM, WiggleGoal.RIGHT_ARM))
return self.wiggle_action_server.set_aborted(
text='arm goal should be {} or {}'.format(
WiggleGoal.LEFT_ARM, WiggleGoal.RIGHT_ARM))
if np.linalg.norm(
np.array([
goal.goal_pose.pose.orientation.x,
goal.goal_pose.pose.orientation.y,
goal.goal_pose.pose.orientation.z,
goal.goal_pose.pose.orientation.w
])) < 0.99:
rospy.logerr('invalid orientation in goal position')
return self.wiggle_action_server.set_aborted(
text='invalid orientation in goal position')
rospy.loginfo('wiggle wiggle wiggle')
movement_duration = goal.timeout.data.to_sec()
wiggle_per_sec = goal.cycle_time
number_of_wiggles = int(movement_duration / wiggle_per_sec)
number_of_wiggle_points = 10
radius_x = goal.upperbound_x
radius_y = goal.upperbound_y
hz = number_of_wiggle_points / wiggle_per_sec
r = rospy.Rate(hz)
tcp_goal_pose = self.transformPose('left_gripper_tool_frame',
goal.goal_pose)
spiral = self.create_spiral(goal_height=tcp_goal_pose.pose.position.z,
radius_x=radius_x,
radius_y=radius_y,
number_of_points=number_of_wiggle_points,
number_of_wiggles=number_of_wiggles)
header = Header()
header.frame_id = 'left_gripper_tool_frame'
pose_list = self.np_array_to_pose_stampeds(spiral,
Quaternion(0, 0, 0, 1),
header)
pose_list = [
deepcopy(self.transformPose('base_footprint', pose))
for pose in pose_list
]
end_pose = self.transformPose('base_footprint', goal.goal_pose)
for i, pose in enumerate(pose_list):
if self.check_for_stop():
return
self.pub_wiggle(pose, goal.arm)
wiggle_feedback = WiggleFeedback()
wiggle_feedback.progress = .9 * (i + 1) / float(len(pose_list))
self.wiggle_action_server.publish_feedback(wiggle_feedback)
r.sleep()
# move back to real goal
time_to_get_to_endpose = rospy.Duration(1.0)
deadline = rospy.get_rostime() + time_to_get_to_endpose
while rospy.get_rostime() < deadline:
if self.check_for_stop():
return
self.pub_wiggle(end_pose, goal.arm)
result = WiggleFeedback()
result.progress = 1.
self.wiggle_action_server.publish_feedback(result)
self.wiggle_action_server.set_succeeded(result)
def check_for_stop(self):
if self.wiggle_action_server.is_preempt_requested():
self.wiggle_action_server.set_preempted(WiggleFeedback(),
'goal canceled')
return True
if self.is_giskard_active:
self.wiggle_action_server.set_aborted(
WiggleFeedback(),
'goal canceled because giskard action server is doing stuff')
return True
return False
def pub_wiggle(self, goal_pose, arm):
if self.save:
self.wiggle_wiggle_wiggle(goal_pose, arm)
self.pose_pub.publish(goal_pose)
def goal_pose_to_cmd(self, goal_pose, arm):
moving_arm_cmd = ArmCommand()
moving_arm_cmd.type = ArmCommand.CARTESIAN_GOAL
moving_arm_cmd.goal_pose = goal_pose
asdf = SemanticFloat64()
asdf.semantics = 'l_rot_error'
asdf.value = 0.0174
moving_arm_cmd.convergence_thresholds.append(asdf)
asdf = SemanticFloat64()
asdf.semantics = 'l_trans_error'
asdf.value = 0.001
moving_arm_cmd.convergence_thresholds.append(asdf)
nonmoving_arm_cmd = ArmCommand()
right_goal = PoseStamped()
right_goal.header.frame_id = 'right_gripper_tool_frame'
right_goal.pose.orientation.w = 1
nonmoving_arm_cmd.type = ArmCommand.CARTESIAN_GOAL
nonmoving_arm_cmd.goal_pose = self.transformPose(
'base_footprint', right_goal)
cmd = WholeBodyCommand()
cmd.type = WholeBodyCommand.STANDARD_CONTROLLER
if arm == WiggleGoal.LEFT_ARM:
cmd.left_ee = moving_arm_cmd
cmd.right_ee = nonmoving_arm_cmd
else:
cmd.left_ee = nonmoving_arm_cmd
cmd.right_ee = moving_arm_cmd
return cmd
def wiggle_wiggle_wiggle(self, goal_pose, arm):
if arm == WiggleGoal.LEFT_ARM or arm == WiggleGoal.RIGHT_ARM:
cmd = self.goal_pose_to_cmd(goal_pose, arm)
self.giskard_goals.publish(cmd)
else:
rospy.logerr('arm goal should be {} or {}'.format(
WiggleGoal.LEFT_ARM, WiggleGoal.RIGHT_ARM))
def create_spiral(self,
goal_height=2.,
radius_x=1.,
radius_y=1.,
number_of_points=10,
number_of_wiggles=10):
wiggle_height = goal_height / number_of_wiggles
wiggles = []
wiggle_template = self.spiral_round(radius_x, radius_y, wiggle_height,
number_of_points)
for i in range(number_of_wiggles):
wiggles.append(wiggle_template +
np.array([0, 0, i * wiggle_height]))
spiral = np.concatenate(wiggles)
return spiral
def spiral_round(self, radius_x, radius_y, height, number_of_points):
points = []
tau = 2 * np.pi
for i, r in enumerate(np.arange(0, tau, tau / number_of_points)):
points.append(
np.array([
np.sin(r) * radius_x,
np.cos(r) * radius_y,
i / float(number_of_points) * float(height)
]))
return np.array(points)
def np_array_to_pose_stampeds(self, nparray, orientation, header):
pose_list = []
for p in nparray:
pose = PoseStamped()
pose.pose.position = Point(*p)
pose.pose.orientation = orientation
pose.header = header
pose_list.append(pose)
return pose_list