diff --git a/beacon_sim-0.0.1-cp38-cp38-manylinux2014_x86_64.whl b/beacon_sim-0.0.1-cp38-cp38-manylinux2014_x86_64.whl index 7a9c1c6..e9d9b22 100755 Binary files a/beacon_sim-0.0.1-cp38-cp38-manylinux2014_x86_64.whl and b/beacon_sim-0.0.1-cp38-cp38-manylinux2014_x86_64.whl differ diff --git a/build_robot_python_wheel.sh b/build_robot_python_wheel.sh index 284154b..580cd86 100755 --- a/build_robot_python_wheel.sh +++ b/build_robot_python_wheel.sh @@ -2,12 +2,13 @@ cd ../robot DAZEL_RC_FILE=".dazelrc_20.04" DAZEL_BAZEL_RC_FILE=".dazel_bazelrc" dazel build --config=python-3.8 //experimental/beacon_sim:beacon_sim_wheel -WHEEL_NAME=`cat bazel-bin/experimental/beacon_sim/beacon_sim_wheel.name` -cp bazel-bin/experimental/beacon_sim/$WHEEL_NAME ../robot-spot +WHEEL_NAME_X86_64=`cat bazel-bin/experimental/beacon_sim/beacon_sim_wheel.name` +cp bazel-bin/experimental/beacon_sim/$WHEEL_NAME_X86_64 ../robot-spot bazel build --config=python-3.8 --platforms=//toolchain:gcc_aarch64 //experimental/beacon_sim:beacon_sim_wheel -WHEEL_NAME=`cat bazel-bin/experimental/beacon_sim/beacon_sim_wheel.name` -cp bazel-bin/experimental/beacon_sim/$WHEEL_NAME ../robot-spot +WHEEL_NAME_AARCH64=`cat bazel-bin/experimental/beacon_sim/beacon_sim_wheel.name` +cp bazel-bin/experimental/beacon_sim/$WHEEL_NAME_AARCH64 ../robot-spot cd ../robot-spot -chmod 777 $WHEEL_NAME +chmod 777 $WHEEL_NAME_X86_64 +chmod 777 $WHEEL_NAME_AARCH64 diff --git a/catkin_ws/src/brm_evanescence/CMakeLists.txt b/catkin_ws/src/brm_evanescence/CMakeLists.txt index 07164a8..0defb32 100644 --- a/catkin_ws/src/brm_evanescence/CMakeLists.txt +++ b/catkin_ws/src/brm_evanescence/CMakeLists.txt @@ -57,6 +57,8 @@ add_service_files( FILES SaveMap.srv LoadMap.srv + CreatePlan.srv + ExecutePlan.srv ) ## Generate actions in the 'action' folder @@ -160,6 +162,7 @@ include_directories( ## in contrast to setup.py, you can choose the destination catkin_install_python(PROGRAMS src/estimation_node.py + src/planning_node.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/catkin_ws/src/brm_evanescence/launch/spot.launch b/catkin_ws/src/brm_evanescence/launch/spot.launch index 75554ac..5b14abf 100644 --- a/catkin_ws/src/brm_evanescence/launch/spot.launch +++ b/catkin_ws/src/brm_evanescence/launch/spot.launch @@ -7,5 +7,5 @@ - + diff --git a/catkin_ws/src/brm_evanescence/launch/stack.launch b/catkin_ws/src/brm_evanescence/launch/stack.launch index b2b6720..ca4fd21 100644 --- a/catkin_ws/src/brm_evanescence/launch/stack.launch +++ b/catkin_ws/src/brm_evanescence/launch/stack.launch @@ -3,5 +3,6 @@ + diff --git a/catkin_ws/src/brm_evanescence/msg/Map.msg b/catkin_ws/src/brm_evanescence/msg/Map.msg index aca79a6..4671157 100644 --- a/catkin_ws/src/brm_evanescence/msg/Map.msg +++ b/catkin_ws/src/brm_evanescence/msg/Map.msg @@ -1,2 +1,3 @@ Header header Landmark[] landmarks +uint8[] estimate_proto diff --git a/catkin_ws/src/brm_evanescence/msg/Plan.msg b/catkin_ws/src/brm_evanescence/msg/Plan.msg new file mode 100644 index 0000000..87d1a98 --- /dev/null +++ b/catkin_ws/src/brm_evanescence/msg/Plan.msg @@ -0,0 +1,3 @@ +Header header +int32[] plan +string roadmap_proto diff --git a/catkin_ws/src/brm_evanescence/package.xml b/catkin_ws/src/brm_evanescence/package.xml index ba16ffe..768f299 100644 --- a/catkin_ws/src/brm_evanescence/package.xml +++ b/catkin_ws/src/brm_evanescence/package.xml @@ -54,6 +54,7 @@ visualization_msgs python3-scipy python3-numpy + actionlib catkin image_proc apriltag_ros diff --git a/catkin_ws/src/brm_evanescence/src/estimation_node.py b/catkin_ws/src/brm_evanescence/src/estimation_node.py index dc1f8fb..b1c6a95 100755 --- a/catkin_ws/src/brm_evanescence/src/estimation_node.py +++ b/catkin_ws/src/brm_evanescence/src/estimation_node.py @@ -49,7 +49,7 @@ class PrioritizedItem: item: AprilTagDetectionArray = field(compare=False) def __init__(self, camera_list): - self._lock = threading.RLock() + self._lock = threading.Lock() self._queue = queue.PriorityQueue() self._last_update_time_from_camera = { c: rtp.RobotTimestamp() for c in camera_list @@ -62,22 +62,28 @@ def insert(self, detections): self._last_update_time_from_camera[camera_name] = detection_time self._queue.put(self.PrioritizedItem(detection_time, detections)) + def _are_samples_available_no_lock(self): + oldest_update_time = min(self._last_update_time_from_camera.values()) + + is_queue_empty = self._queue.empty() + if not is_queue_empty: + is_last_update_recent_enough = oldest_update_time >= self._queue.queue[0].receive_time + return is_last_update_recent_enough + return False + def are_samples_available(self): with self._lock: - oldest_update_time = min(self._last_update_time_from_camera.values()) - return ( - not self._queue.empty() - and oldest_update_time >= self._queue.queue[0].receive_time - ) + result = self._are_samples_available_no_lock() + return result def pop(self): with self._lock: - if not self.are_samples_available(): + if not self._are_samples_available_no_lock(): return None - return self._queue.get().item + return self._queue.get_nowait().item -def robot_time_from_ros_time(stamp: ros.Time): +def robot_time_from_ros_time(stamp: rospy.Time): return ( rtp.RobotTimestamp() + rtp.as_duration(stamp.secs) @@ -163,6 +169,7 @@ def stamped_transform_from_point2(pt_in_b, b, a, ros_time): def compute_observations(detections, tf_buffer): observations = [] + timeout = rospy.Duration(0.5) for detection in detections: stamped_pose = detection.pose camera_frame = stamped_pose.header.frame_id @@ -170,9 +177,10 @@ def compute_observations(detections, tf_buffer): id = detection.id[0] stamp = stamped_pose.header.stamp time_of_validity = robot_time_from_ros_time(stamp) - timeout = rospy.Duration(0.25) - - body_from_camera = tf_buffer.lookup_transform(BODY_FRAME, camera_frame, stamp, timeout) + try: + body_from_camera = tf_buffer.lookup_transform(BODY_FRAME, camera_frame, stamp, timeout) + except: + continue camera_from_tag = detection.pose.pose @@ -264,9 +272,9 @@ def create_obs_viz(observations, camera_name, ekf_tov): marker.pose.orientation.w = 1.0 - marker.scale.x = 0.1 - marker.scale.y = 0.1 - marker.scale.z = 0.1 + marker.scale.x = 0.2 + marker.scale.y = 0.2 + marker.scale.z = 0.2 is_old = obs_and_time[0] - ekf_tov < -rtp.as_duration(0.5) @@ -406,11 +414,6 @@ def create_viz_msg(ekf): return viz_msg -def observation_callback(detections, observations_queue, queue_lock): - with queue_lock: - observations_queue.append(detections) - - def tick_estimator( timer_event, observations_queue, @@ -425,9 +428,9 @@ def tick_estimator( while observations_queue.are_samples_available(): detections_msg = observations_queue.pop() new_detections = compute_observations(detections_msg.detections, tf_buffer) + new_detections = sorted(new_detections, key=lambda x: x[0]) # Update the detections visualization - new_detections = sorted(new_detections, key=lambda x: x[0]) viz_publisher.publish( create_obs_viz( new_detections, @@ -458,6 +461,7 @@ def tick_estimator( # publish the estimate tf_message = create_tf_msg(ekf) + rospy.loginfo(tf_message) if tf_message is not None: tf_publisher.publish(tf_message) @@ -481,6 +485,8 @@ def publish_map(timer_event, ekf, map_publisher, viz_publisher): landmark = Landmark(id = beacon_id, x=beacon_in_local[0], y=beacon_in_local[1]) map_out.landmarks.append(landmark) + map_out.estimate_proto = ekf.estimate.to_proto_string() + map_publisher.publish(map_out) marker_out = viz.MarkerArray() @@ -563,8 +569,8 @@ def main(): beacon_pos_process_noise_m_per_rt_s=0.001, range_measurement_noise_m=0.1, bearing_measurement_noise_rad=0.02, - on_map_load_position_uncertainty_m=0.1, - on_map_load_heading_uncertainty_rad=0.01, + on_map_load_position_uncertainty_m=10.0, + on_map_load_heading_uncertainty_rad=0.4, ) camera_list = ["frontleft", "frontright", "left", "back", "right"] @@ -575,17 +581,20 @@ def main(): rospy.Service(f'{rospy.get_name()}/save_map', SaveMap, lambda req: save_map_handler(req, esp.EkfSlam(ekf))) rospy.Service(f'{rospy.get_name()}/load_map', LoadMap, lambda req: load_map_handler(req, ekf)) + def obs_callback(data): + observations_queue.insert(data) + for camera in camera_list: topic_name = f"/spot/apriltag/{camera}/tag_detections" tag_detection_subscribers.append( rospy.Subscriber( topic_name, AprilTagDetectionArray, - lambda data: observations_queue.insert(data), + obs_callback, ) ) - rospy.Timer( + estimator_tick = rospy.Timer( rospy.Duration(0.05), lambda timer_event: tick_estimator( timer_event, @@ -598,7 +607,7 @@ def main(): ), ) - rospy.Timer( + map_publish = rospy.Timer( rospy.Duration(1.0), lambda timer_event: publish_map( timer_event, diff --git a/catkin_ws/src/brm_evanescence/src/planning_node.py b/catkin_ws/src/brm_evanescence/src/planning_node.py new file mode 100644 index 0000000..29a969e --- /dev/null +++ b/catkin_ws/src/brm_evanescence/src/planning_node.py @@ -0,0 +1,428 @@ + +from __future__ import annotations + +import threading + +import spot_msgs.msg +import rospy +import tf2_ros +import visualization_msgs.msg as viz +import geometry_msgs +import actionlib +import time +import numpy as np + +from brm_evanescence.srv import CreatePlanResponse, CreatePlan, CreatePlanRequest +from brm_evanescence.srv import ExecutePlanResponse, ExecutePlan +from brm_evanescence.msg import Map + +import common.liegroups.se2_python as se2 +import experimental.beacon_sim.beacon_potential_python as bpp +import planning.probabilistic_road_map_python as prmp +import experimental.beacon_sim.belief_road_map_planner_python as brmp +import experimental.beacon_sim.ekf_slam_python as esp +from scipy.spatial.transform import Rotation as R + +from typing import NamedTuple + +BODY_FRAME = "flat_body" +MAP_FRAME = "map" + + +class Plan(NamedTuple): + time_of_validity: rospy.Time + nodes: list[int] + + +EKF_CONFIG = esp.EkfSlamConfig( + max_num_beacons=10, + initial_beacon_uncertainty_m=100.0, + along_track_process_noise_m_per_rt_meter=0.02, + cross_track_process_noise_m_per_rt_meter=0.02, + pos_process_noise_m_per_rt_s=0.0001, + heading_process_noise_rad_per_rt_meter=0.0001, + heading_process_noise_rad_per_rt_s=0.00001, + beacon_pos_process_noise_m_per_rt_s=0.001, + range_measurement_noise_m=0.1, + bearing_measurement_noise_rad=0.02, + on_map_load_position_uncertainty_m=0.1, + on_map_load_heading_uncertainty_rad=0.01, +) + +MAX_SENSOR_RANGE_M = 2.5 +TIMEOUT = None +MAX_NUM_TRANSFORMS = 1000 +BELIEF_ROADMAP_OPTIONS = brmp.BeliefRoadMapOptions( + max_sensor_range_m=MAX_SENSOR_RANGE_M, + uncertainty_tolerance=None, + max_num_edge_transforms=MAX_NUM_TRANSFORMS, + timeout=TIMEOUT +) + +NUM_WORLD_SAMPLES = 100 +SEED = 0 +EXPECTED_BELIEF_ROADMAP_OPTIONS = brmp.ExpectedBeliefRoadMapOptions( + NUM_WORLD_SAMPLES, SEED, BELIEF_ROADMAP_OPTIONS +) + +MAX_NUM_COMPONENTS = 100 +AGGREGATION_METHOD = brmp.LandmarkBeliefRoadMapOptions.ExpectedDeterminant() +# AGGREGATION_METHOD = brmp.LandmarkBeliefRoadMapOptions.ValueAtRiskDeterminant(0.95) +SAMPLED_BELIEF = brmp.LandmarkBeliefRoadMapOptions.SampledBeliefOptions( + MAX_NUM_COMPONENTS, + SEED, +) +LANDMARK_BELIEF_ROADMAP_OPTIONS = brmp.LandmarkBeliefRoadMapOptions( + MAX_SENSOR_RANGE_M, + AGGREGATION_METHOD, + SAMPLED_BELIEF, + TIMEOUT, +) + + +def run_optimistic_brm_planner(road_map, potential, ekf): + potential = bpp.BeaconPotential() + result = brmp.compute_belief_road_map_plan( + road_map, ekf, potential, BELIEF_ROADMAP_OPTIONS + ) + return result.nodes + + +def run_expected_brm_planner(road_map, potential, ekf): + result = brmp.compute_expected_belief_road_map_plan( + road_map, ekf, potential, EXPECTED_BELIEF_ROADMAP_OPTIONS + ) + return result.nodes + + +def run_landmark_brm_planner(road_map, potential, ekf): + result = brmp.compute_landmark_belief_road_map_plan( + road_map, ekf, potential, LANDMARK_BELIEF_ROADMAP_OPTIONS + ) + return result.nodes + + +def robot_se2_from_stamped_transform(b_from_a_msg): + origin = np.zeros(3) + unit_x = np.zeros(3) + unit_x[0] = 1.0 + + b_from_a_rot = R.from_quat( + [ + b_from_a_msg.transform.rotation.x, + b_from_a_msg.transform.rotation.y, + b_from_a_msg.transform.rotation.z, + b_from_a_msg.transform.rotation.w, + ] + ) + + b_from_a_trans = np.array( + [ + b_from_a_msg.transform.translation.x, + b_from_a_msg.transform.translation.y, + b_from_a_msg.transform.translation.z, + ] + ) + + origin_in_b = b_from_a_rot.apply(origin) + b_from_a_trans + x_in_b = b_from_a_rot.apply(unit_x) + b_from_a_trans + + theta = np.arctan2(x_in_b[1] - origin_in_b[1], x_in_b[0] - origin_in_b[0]) + return se2.SE2(theta, np.array([origin_in_b[0], origin_in_b[1]])) + + +class PlanningNode: + def __init__(self): + rospy.init_node("planning_node") + + rospy.Subscriber("/map", Map, self.map_callback) + self._viz_publisher = rospy.Publisher("/plan_visualization", viz.MarkerArray, queue_size=16) + rospy.Service( + f"{rospy.get_name()}/create_plan", CreatePlan, self.handle_plan_request + ) + + rospy.Service( + f"{rospy.get_name()}/execute_plan", ExecutePlan, self.handle_plan_execution_request + ) + self._tf_buffer = tf2_ros.Buffer() + self._tf_listener = tf2_ros.TransformListener(self._tf_buffer) + + self._traj_client = actionlib.SimpleActionClient('/spot/trajectory', spot_msgs.msg.TrajectoryAction) + + self._map_lock = threading.Lock() + self._map = None + + self._plan = None + self._road_map = None + + def map_callback(self, data): + with self._map_lock: + self._map = data + + def visualize_road_map(self, road_map): + marker_out = viz.MarkerArray() + + pt_idxs = list(range(len(road_map.points()))) + if road_map.has_start_goal(): + pt_idxs += [road_map.START_IDX, road_map.GOAL_IDX] + + # Visualize the nodes + for pt_idx in pt_idxs: + marker = viz.Marker() + pt = road_map.point(pt_idx) + marker.header.frame_id = "map" + + marker.ns = "road_map_nodes" + marker.id = pt_idx + marker.type = viz.Marker.CUBE + marker.action = viz.Marker.ADD + marker.pose.position.x = pt[0] + marker.pose.position.y = pt[1] + marker.pose.orientation.w = 1.0 + + marker.scale.x = 0.1 + marker.scale.y = 0.1 + marker.scale.z = 0.1 + + marker.color.r = 0.1 + marker.color.g = 0.6 + marker.color.b = 0.6 + marker.color.a = 1.0 + + if pt_idx == road_map.START_IDX: + marker.color.r = 0.6 + marker.color.g = 0.1 + marker.color.b = 0.6 + marker.color.a = 1.0 + + elif pt_idx == road_map.GOAL_IDX: + marker.color.r = 0.1 + marker.color.g = 0.1 + marker.color.b = 0.6 + marker.color.a = 1.0 + + marker_out.markers.append(marker) + + marker = viz.Marker() + pt = road_map.point(pt_idx) + marker.header.frame_id = "map" + + marker.ns = "road_map_nodes_labels" + marker.id = pt_idx + marker.type = viz.Marker.TEXT_VIEW_FACING + marker.action = viz.Marker.ADD + marker.pose.position.x = pt[0] + 0.15 + marker.pose.position.y = pt[1] + marker.pose.orientation.w = 1.0 + TEXT_HEIGHT_M = 0.1 + marker.scale.z = TEXT_HEIGHT_M + marker.color.r = 0.8 + marker.color.g = 0.8 + marker.color.b = 0.8 + marker.color.a = 1.0 + + marker.text = f'{pt_idx}' + + if pt_idx == road_map.START_IDX: + marker.text = 'Start' + marker.pose.position.z = TEXT_HEIGHT_M + + elif pt_idx == road_map.GOAL_IDX: + marker.text = 'Goal' + marker.pose.position.z = TEXT_HEIGHT_M + + marker_out.markers.append(marker) + + # Visualize the edges + edge_marker = viz.Marker() + edge_marker.header.frame_id = "map" + edge_marker.ns = "road_map_edges" + edge_marker.id = 0 + edge_marker.type = viz.Marker.LINE_LIST + edge_marker.action = viz.Marker.ADD + edge_marker.pose.orientation.w = 1.0 + + edge_marker.scale.x = 0.01 + edge_marker.color.r = 0.6 + edge_marker.color.g = 0.1 + edge_marker.color.b = 0.1 + edge_marker.color.a = 1.0 + + for pt_idx in pt_idxs: + pt_loc = road_map.point(pt_idx) + for neighbor_idx, neighbor_loc in road_map.neighbors(pt_idx): + if neighbor_idx < pt_idx: + continue + edge_marker.points.append( + geometry_msgs.msg.Point(x=pt_loc[0], y=pt_loc[1], z=0.0) + ) + edge_marker.points.append( + geometry_msgs.msg.Point(x=neighbor_loc[0], y=neighbor_loc[1], z=0.0) + ) + marker_out.markers.append(edge_marker) + + self._viz_publisher.publish(marker_out) + + def visualize_plan(self, road_map, plan): + plan_marker = viz.Marker() + plan_marker.header.frame_id = "map" + plan_marker.ns = "plan_edges" + plan_marker.id = 0 + plan_marker.type = viz.Marker.LINE_STRIP + plan_marker.action = viz.Marker.ADD + plan_marker.pose.position.z = 0.1 + plan_marker.pose.orientation.w = 1.0 + + plan_marker.scale.x = 0.02 + plan_marker.color.r = 0.1 + plan_marker.color.g = 0.7 + plan_marker.color.b = 0.1 + plan_marker.color.a = 1.0 + + for pt_idx in plan: + pt_loc = road_map.point(pt_idx) + plan_marker.points.append( + geometry_msgs.msg.Point(x=pt_loc[0], y=pt_loc[1], z=0.0) + ) + + self._viz_publisher.publish(viz.MarkerArray([plan_marker])) + + def handle_plan_request(self, req): + rospy.loginfo(req) + + # Get the latest estimate + with self._map_lock: + if self._map is None or self._map.header.stamp == rospy.Time(): + return CreatePlanResponse(success=False) + + ekf_estimate = esp.EkfSlamEstimate.from_proto_string( + self._map.estimate_proto + ) + ekf = esp.EkfSlam(EKF_CONFIG, ekf_estimate.time_of_validity) + ekf.estimate = ekf_estimate + + # Load the beacon potential + with open(req.beacon_potential_path, "rb") as file_in: + beacon_potential = bpp.BeaconPotential.from_proto_string(file_in.read()) + + # Load the road map + with open(req.road_map_path, "rb") as file_in: + road_map = prmp.RoadMap.from_proto_string(file_in.read()) + + goal_loc = road_map.point(req.goal_node) + start_loc = ekf.estimate.local_from_robot().translation() + + CONNECTION_RADIUS_M = 2 + road_map.add_start_goal( + prmp.StartGoalPair(start_loc, goal_loc, CONNECTION_RADIUS_M) + ) + + # call the appropriate planner + if req.planning_method == CreatePlanRequest.OPTIMISTIC_BRM: + plan = run_optimistic_brm_planner(road_map, beacon_potential, ekf) + elif req.planning_method == CreatePlanRequest.EXPECTED_BRM: + plan = run_expected_brm_planner(road_map, beacon_potential, ekf) + elif req.planning_method == CreatePlanRequest.LANDMARK_BRM: + plan = run_landmark_brm_planner(road_map, beacon_potential, ekf) + else: + rospy.loginfo( + f"Unknown Planning Method: {req.planning_method} not in [{CreatePlanRequest.OPTIMISTIC_BRM}, {CreatePlanRequest.EXPECTED_BRM}, {CreatePlanRequest.LANDMARK_BRM}]" + ) + return CreatePlanResponse(success=False) + + rospy.loginfo(f"Plan: {plan}") + self._road_map = road_map + self._plan = Plan( + time_of_validity=rospy.Time.now(), + nodes=plan + ) + + self.visualize_road_map(self._road_map) + self.visualize_plan(self._road_map, plan) + + return CreatePlanResponse(success=True) + + def handle_plan_execution_request(self, req): + if self._plan is None: + return ExecutePlanResponse(success=False) + + trajectory = spot_msgs.msg.TrajectoryGoal() + trajectory.precise_positioning = True + trajectory.duration.data = rospy.Duration(20.0) + trajectory.target_pose.header.frame_id = BODY_FRAME + + timeout = rospy.Duration(1.0) + for node_id in self._plan.nodes[1:]: + # Turn towards the next point + nowish = rospy.Time.now() - rospy.Duration(1.0) + map_from_robot = None + while True: + try: + map_from_robot_ros = self._tf_buffer.lookup_transform( + MAP_FRAME, BODY_FRAME, nowish, timeout) + map_from_robot = robot_se2_from_stamped_transform(map_from_robot_ros) + break + except Exception as e: + rospy.loginfo(e) + continue + + + goal_in_map = self._road_map.point(node_id) + goal_in_body = map_from_robot.inverse() @ goal_in_map + if np.linalg.norm(goal_in_body) < 0.5: + # We are near the next node, so we skip it + continue + theta = np.arctan2(goal_in_body[1], goal_in_body[0]) + robot_from_goal_facing_robot = R.from_rotvec(np.array([0, 0, theta])) + robot_from_goal_facing_robot_quat = robot_from_goal_facing_robot.as_quat() + + trajectory.target_pose.pose.position.x = 0.0 + trajectory.target_pose.pose.position.y = 0.0 + trajectory.target_pose.pose.position.z = 0.0 + trajectory.target_pose.pose.orientation.x = robot_from_goal_facing_robot_quat[0] + trajectory.target_pose.pose.orientation.y = robot_from_goal_facing_robot_quat[1] + trajectory.target_pose.pose.orientation.z = robot_from_goal_facing_robot_quat[2] + trajectory.target_pose.pose.orientation.w = robot_from_goal_facing_robot_quat[3] + + self._traj_client.send_goal(trajectory) + result = self._traj_client.wait_for_result(rospy.Duration(10.0)) + time.sleep(2.0) + + # Move towards the goal + nowish = rospy.Time.now() - rospy.Duration(1.0) + while True: + try: + map_from_robot_ros = self._tf_buffer.lookup_transform( + MAP_FRAME, BODY_FRAME, nowish, timeout) + map_from_robot = robot_se2_from_stamped_transform(map_from_robot_ros) + break + except Exception as e: + rospy.loginfo(e) + continue + goal_in_body = map_from_robot.inverse() @ goal_in_map + + + trajectory.target_pose.pose.position.x = goal_in_body[0] + trajectory.target_pose.pose.position.y = goal_in_body[1] + trajectory.target_pose.pose.position.z = 0.0 + trajectory.target_pose.pose.orientation.x = 0.0 + trajectory.target_pose.pose.orientation.y = 0.0 + trajectory.target_pose.pose.orientation.z = 0.0 + trajectory.target_pose.pose.orientation.w = 1.0 + + self._traj_client.send_goal(trajectory) + result = self._traj_client.wait_for_result(rospy.Duration(10.0)) + time.sleep(2.0) + + + return ExecutePlanResponse(success=True) + + + +if __name__ == "__main__": + planning_node = PlanningNode() + + try: + rospy.spin() + except KeyboardInterrupt: + ... diff --git a/catkin_ws/src/brm_evanescence/srv/CreatePlan.srv b/catkin_ws/src/brm_evanescence/srv/CreatePlan.srv new file mode 100644 index 0000000..68d34e5 --- /dev/null +++ b/catkin_ws/src/brm_evanescence/srv/CreatePlan.srv @@ -0,0 +1,10 @@ +string OPTIMISTIC_BRM=optimistic +string EXPECTED_BRM=expected +string LANDMARK_BRM=landmark + +string beacon_potential_path +string road_map_path +string planning_method +int32 goal_node +--- +bool success diff --git a/catkin_ws/src/brm_evanescence/srv/ExecutePlan.srv b/catkin_ws/src/brm_evanescence/srv/ExecutePlan.srv new file mode 100644 index 0000000..410e0f9 --- /dev/null +++ b/catkin_ws/src/brm_evanescence/srv/ExecutePlan.srv @@ -0,0 +1,2 @@ +--- +bool success diff --git a/estimator_topic_list.txt b/estimator_topic_list.txt new file mode 100644 index 0000000..e3026be --- /dev/null +++ b/estimator_topic_list.txt @@ -0,0 +1,4 @@ +/clock +/map +/tf +/tf_static diff --git a/robot_room_beacon_potential.pb b/robot_room_beacon_potential.pb new file mode 100644 index 0000000..cc12a58 Binary files /dev/null and b/robot_room_beacon_potential.pb differ diff --git a/robot_room_map.pb b/robot_room_map.pb new file mode 100644 index 0000000..55b3847 Binary files /dev/null and b/robot_room_map.pb differ diff --git a/robot_room_road_map.pb b/robot_room_road_map.pb new file mode 100644 index 0000000..389cd56 Binary files /dev/null and b/robot_room_road_map.pb differ diff --git a/run_in_docker.sh b/run_in_docker.sh deleted file mode 100755 index e69de29..0000000 diff --git a/run_me_in_docker.sh b/run_me_in_docker.sh index 552d705..f6ad443 100755 --- a/run_me_in_docker.sh +++ b/run_me_in_docker.sh @@ -2,7 +2,9 @@ CURDIR=$(pwd) -pip install --force-reinstall beacon_sim*.whl +ARCH=$(uname -m) + +pip install --force-reinstall beacon_sim*${ARCH}.whl rosdep install brm_evanescence --ignore-src -y cd $(dirname $0)/catkin_ws/src/spot_ros rosdep install --from-paths spot_cam spot_description spot_driver \