Added footstep visualization rviz

soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/foot_step_planner.py

@@ -28,6 +28,7 @@ def __init__(self, robot_model: str, parameters: dict, funct_time, debug: bool =
         self.trajectory = None
         self.tasks = None
         self.solver = None
+        self.footstep_polygons = []
         if self.debug:
             # Starting Meshcat viewer
             self.viz = robot_viz(self.robot)
@@ -211,23 +212,24 @@ def step(self, t: float):
 
     def update_viz(self, supports: List[placo.Supports], trajectory: placo.WalkTrajectory):
         if self.debug:
+            self.footstep_polygons = []
             # Drawing footsteps
             footsteps_viz(supports)  # TODO look for step vis
             # TODO convert for ros
-            # steps = len(supports)
-            # T = np.eye(4)
-            # k = 0
-            # for footstep in supports:
-            #     k += 1
-            #     polygon = [(T @ [*xy, 0, 1])[:3] for xy in footstep.support_polygon()]
-            #     polygon = np.array([*polygon, polygon[-1]])
-            #
-            #     polygon[:, 1] -= self.init_foot[1, 3]/2
-            #     p.addUserDebugLine(polygon[0], polygon[1], [1, 0,0], lifeTime=1)
-            #     p.addUserDebugLine(polygon[1], polygon[2], [0, 1, 0], lifeTime=1)
-            #     p.addUserDebugLine(polygon[2], polygon[3], [0, 0, 1], lifeTime=1)
-            #     p.addUserDebugLine(polygon[0], polygon[3], [0, 1, 1], lifeTime=1)
-            #     print("fsd")
+            steps = len(supports)
+            T = np.eye(4)
+            k = 0
+            for footstep in supports:
+                k += 1
+                polygon = [(T @ [*xy, 0, 1])[:3] for xy in footstep.support_polygon()]
+                polygon = np.array([*polygon, polygon[-1]])
+                polygon[:, 1] -= self.init_foot[1, 3]/2
+                self.footstep_polygons.append(polygon)
+                # p.addUserDebugLine(polygon[0], polygon[1], [1, 0, 0], lifeTime=1)
+                # p.addUserDebugLine(polygon[1], polygon[2], [0, 1, 0], lifeTime=1)
+                # p.addUserDebugLine(polygon[2], polygon[3], [0, 0, 1], lifeTime=1)
+                # p.addUserDebugLine(polygon[0], polygon[3], [0, 1, 1], lifeTime=1)
+                print("fsd")
             # Drawing planned CoM trajectory on the ground
             coms = [[*trajectory.get_p_world_CoM(t)[:2], 0.0] for t in np.linspace(trajectory.t_start, trajectory.t_end, 100)]
             line_viz("CoM_trajectory", np.array(coms), 0xFFAA00)

soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/navigator.py

@@ -16,7 +16,7 @@
 
 # TODO change to trajectory controller
 class Navigator:
-    def __init__(self, world: PybulletWorld, bez: Bez, imu_feedback_enabled: bool = False, record_walking_metrics: bool = True):
+    def __init__(self, world: PybulletWorld, bez: Bez, imu_feedback_enabled: bool = False, record_walking_metrics: bool = False):
         self.world = world
         self.bez = bez
         self.imu_feedback_enabled = imu_feedback_enabled

soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/walk_engine_ros/navigator_ros.py

@@ -1,5 +1,8 @@
 import rospy
-from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import PoseStamped, Point
+from std_msgs.msg import ColorRGBA
+# from pybullet_examples.getClosestPoints import colorRGB
+from visualization_msgs.msg import Marker, MarkerArray
 from soccer_pycontrol.model.model_ros.bez_ros import BezROS
 from soccer_pycontrol.walk_engine.foot_step_planner import FootStepPlanner
 from soccer_pycontrol.walk_engine.navigator import Navigator
@@ -9,8 +12,9 @@
 
 
 class NavigatorRos(Navigator):
-    def __init__(self, bez: BezROS, imu_feedback_enabled: bool = False):
+    def __init__(self, bez: BezROS, imu_feedback_enabled: bool = False, record_walking_metrics: bool = False):
         self.imu_feedback_enabled = imu_feedback_enabled
+        self.record_walking_metrics = record_walking_metrics
         self.bez = bez
 
         self.foot_step_planner = FootStepPlanner(self.bez.robot_model, self.bez.parameters, rospy.get_time)
@@ -44,6 +48,7 @@ def __init__(self, bez: BezROS, imu_feedback_enabled: bool = False):
 
         self.error_tol = 0.05  # in m TODO add as a param and in the ros version
         self.position_subscriber = rospy.Subscriber(self.bez.ns + "goal", PoseStamped, self.goal_callback)
+        self.polygon_publisher = rospy.Publisher(self.bez.ns + "polygons", MarkerArray, queue_size=1) # set queue_size to 1, look at marker
         self.goal = PoseStamped()
 
     def goal_callback(self, pose: PoseStamped) -> None:
@@ -61,3 +66,34 @@ def goal_callback(self, pose: PoseStamped) -> None:
     def wait(self, steps: int):
         for i in range(steps):
             rospy.sleep(self.foot_step_planner.DT)
+
+    def walk_loop(self, t):
+        t= super(NavigatorRos, self).walk_loop(t)
+        polygons = MarkerArray()
+        for i, polygon in enumerate(self.foot_step_planner.footstep_polygons):
+            marker = Marker()
+            marker.header.frame_id = "map" # or map
+            marker.header.stamp = rospy.Time.now()
+            marker.type = marker.LINE_STRIP
+            marker.id = i
+            marker.action = marker.ADD
+            marker.scale.x = 0.01
+            # marker.color.b = 1.0
+            marker.color.a = 1.0
+            r = ColorRGBA(1, 0, 0, 1)
+            g = ColorRGBA(0, 1, 0, 1)
+            b = ColorRGBA(0, 0, 1, 1)
+            p1 = Point(polygon[0][0], polygon[0][1], polygon[0][2])
+            p2 = Point(polygon[1][0], polygon[1][1], polygon[1][2])
+            p3 = Point(polygon[2][0], polygon[2][1], polygon[2][2])
+            p4 = Point(polygon[3][0], polygon[3][1], polygon[3][2])
+            p5 = Point(polygon[4][0], polygon[4][1], polygon[4][2])
+
+            marker.points.extend([p1,p2,p2,p3,p3,p4,p4,p5,p5,p1])
+            marker.colors.extend([r,r,g,g,b,b,g,g,g,g])
+
+            polygons.markers.append(marker)
+
+        self.polygon_publisher.publish(polygons)
+        return t
+