Bug fix after addressing comments

Author: MultyXu

spot_tools/src/spot_skills/navigation_utils.py

@@ -160,11 +160,12 @@ def follow_trajectory_continuous(
         # if mid_level_planner is not None:
         # update path every (couple?) loop
         mlp_success, planning_output = mid_level_planner.plan_path(waypoints_list[:, :2])
-        path = planning_output['path_shapely']
-        path_wp = planning_output['path_waypoints_metric']
-        target_point_metric = planning_output['target_point_metric']
-        
+        path = planning_output.path_shapely
+        path_wp = planning_output.path_waypoints_metric
+        target_point_metric = planning_output.target_point_metric
+
         if feedback is not None and target_point_metric is not None:
+            feedback.print("INFO", f"target_point_metric: {target_point_metric}")
             feedback.path_follow_MLP_feedback(path_wp, target_point_metric)
 
         if not mlp_success:

spot_tools_ros/src/spot_tools_ros/fake_path_publisher.py

@@ -52,6 +52,76 @@ def __init__(self, goal_x, goal_y, map_frame, robot_name, follow_robot=False, pu
             self.timer = self.create_timer(1.0, self.publish_once)
             self.get_logger().info(f"Publishing path to goal ({goal_x}, {goal_y}) once")
 
+    def to_viz_msg_follow(self, action, marker_ns):
+            points = []
+            for p in action.path.poses:
+                pt = Point()
+                pt.x = p.pose.position.x
+                pt.y = p.pose.position.y
+                pt.z = p.pose.position.z
+                points.append(pt)
+            m = Marker()
+            m.header.frame_id = self.map_frame
+            m.header.stamp = gtm()
+            m.ns = marker_ns
+            m.id = 0
+            m.type = m.LINE_STRIP
+            m.action = m.ADD
+            m.pose.orientation.w = 1.0
+            m.scale.x = 0.2
+            m.scale.y = 0.2
+            m.color.a = 1.0
+            m.color.r = 0.0
+            m.color.g = 1.0
+            m.color.b = 0.0
+            m.points = points
+
+            start = Marker()
+            start.header.frame_id = self.map_frame
+            start.header.stamp = gtm()
+            start.ns = marker_ns
+            start.id = 1
+            start.type = m.SPHERE
+            start.action = m.ADD
+            start.pose.orientation.w = 1.0
+            start.scale.x = 0.4
+            start.scale.y = 0.4
+            start.scale.z = 0.4
+            start.color.a = 0.5
+            start.color.r = 1.0
+            start.color.g = 0.0
+            start.color.b = 0.0
+            start.pose.position.x = points[0].x
+            start.pose.position.y = points[0].y
+            start.pose.position.z = points[0].z
+
+            end = Marker()
+            end.header.frame_id = self.map_frame
+            end.header.stamp = gtm()
+            end.ns = marker_ns
+            end.id = 2
+            end.type = m.SPHERE
+            end.action = m.ADD
+            end.pose.orientation.w = 1.0
+            end.scale.x = 0.4
+            end.scale.y = 0.4
+            end.scale.z = 0.4
+            end.color.a = 0.5
+            end.color.r = 0.0
+            end.color.g = 0.0
+            end.color.b = 1.0
+            end.pose.position.x = points[-1].x
+            end.pose.position.y = points[-1].y
+            end.pose.position.z = points[-1].z
+
+            return [m, start, end]
+
+
+    def to_viz_msg(self, action, marker_ns):
+        ma = MarkerArray()
+        for ix, a in enumerate(action.actions):
+            ma.markers += self.to_viz_msg_follow(a, marker_ns + f"/{ix}")
+        return ma
 
 
     def generate_waypoints(self, start_x, start_y, start_yaw, num_waypoints=10):
@@ -71,7 +141,7 @@ def publish_path(self):
         """Publish path from current robot pose to goal"""
         try:
             # Get current robot pose
-            robot_pose, robot_yaw = get_tf_pose(self.tf_buffer, self.map_frame, self.robot_frame)
+            robot_pose, (roll, pitch, robot_yaw) = get_tf_pose(self.tf_buffer, self.map_frame, self.robot_frame, get_euler=True)
 
             # Generate waypoints
             if self.follow_robot:
@@ -117,7 +187,8 @@ def publish_path(self):
             msg.actions.append(action_msg)
 
             self.publisher.publish(msg)
-            self.viz_publisher.publish(to_viz_msg(msg, self.robot_name))
+            # self.viz_publisher.publish(to_viz_msg(msg, self.robot_name)) # somehow throw an error about the msg type, it has a '_' before the actual msg name
+            self.viz_publisher.publish(self.to_viz_msg(msg, self.robot_name))
             self.get_logger().info(f"Published path with {len(waypoints)} waypoints")
 
         except Exception as e:
@@ -162,4 +233,4 @@ def main():
 
 
 if __name__ == "__main__":
-    main()
+    main()

spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py

@@ -29,7 +29,7 @@
 from spot_skills.detection_utils import YOLODetector
 from std_msgs.msg import Bool, String
 from visualization_msgs.msg import Marker, MarkerArray
-
+from shapely.geometry import Point
 
 from spot_tools_ros.fake_spot_ros import FakeSpotRos
 from spot_tools_ros.occupancy_grid_ros_updater import OccupancyGridROSUpdater
@@ -149,7 +149,7 @@ def path_following_progress_feedback(self, progress_point, target_point):
 
     def path_follow_MLP_feedback(self, path, target_point_metric):        
         self.mlp_path_publisher.publish(waypoints_to_path(self.odom_frame, path)) # TODO: parameterize frame name
-        target_point_metric_flattened = [p[0] for p in target_point_metric]
+        target_point_metric_flattened = Point([p[0] for p in target_point_metric[:3]])
 
         pts = [target_point_metric_flattened]
         namespaces = ["projected target point"]

spot_tools_ros/src/spot_tools_ros/utils.py

@@ -68,7 +68,7 @@ def pose_to_homo(pose, quat):
     return homo_mat
 
 
-def get_tf_pose(tf_buffer, parent_frame: str, child_frame: str):
+def get_tf_pose(tf_buffer, parent_frame: str, child_frame: str, get_euler=False):
     """
     Looks up the transform from parent_frame to child_frame and returns [x, y, z, yaw].
 
@@ -91,6 +91,8 @@ def get_tf_pose(tf_buffer, parent_frame: str, child_frame: str):
         quat = [rotation.x, rotation.y, rotation.z, rotation.w]
         roll, pitch, yaw = tf_transformations.euler_from_quaternion(quat)
 
+        if get_euler:
+            return np.array([translation.x, translation.y, translation.z]), (roll, pitch, yaw)
         return np.array([translation.x, translation.y, translation.z]), rotation
 
     except tf2_ros.TransformException as e: