changes in the lab to make the Jackal demo work again.

mpc_planner_jackal/README.md

@@ -9,6 +9,12 @@ Several additional packages are required to run the planner in the lab. These ar
 
 ---
 
+### Demo Instructions
+
+1. Connect to `mrl-wifi-5g`.
+2. `ip a`, set `ROS_IP` in `connect_to_jackal.sh`.
+3. `Ctrl + Shift + B` -> `Jackal: Run Real-World Jackal`
+
 ### Notes
 - The launch file configures the number of pedestrians, which needs to be correct for any pedestrian to be detected.
 - The launch file also allows static obstacles with markers to be added (as dynamic obstacle with zero velocity).

mpc_planner_jackal/config/guidance_planner.yaml

@@ -9,10 +9,10 @@ guidance_planner:
 
   homotopy:
     n_paths: 4 # Number of guidance trajectories
-    comparison_function: Homology # Homology (default) Winding or UVD
+    comparison_function: Winding # Homology (default) Winding or UVD
     winding:
       pass_threshold: 0.87 #1.74 # half of pi
-      use_non_passing: true
+      use_non_passing: false
     use_learning: false
     track_selected_homology_only: false
 
@@ -27,8 +27,8 @@ guidance_planner:
     timeout: 10 # Timeout for PRM sampling [ms]
     margin: 0.0 # [m] sampled outside of goals 
 
-  max_velocity: 7.0 # Maximum velocity of connections between nodes
-  max_acceleration: 7.0 # Maximum velocity of connections between nodes
+  max_velocity: 3.0 # Maximum velocity of connections between nodes
+  max_acceleration: 3.0 # Maximum velocity of connections between nodes
   connection_filters:
     forward: true
     velocity: true

mpc_planner_jackal/config/settings.yaml

@@ -4,8 +4,8 @@ integrator_step: 0.2
 n_discs: 1
 
 solver_settings:
-  solver: "acados" # acados or forces
-  # solver: "forces" # acados or forces
+  # solver: "acados" # acados or forces
+  solver: "forces" # acados or forces
   acados:
     iterations: 10
     solver_type: SQP_RTI # SQP_RTI (default) or SQP
@@ -70,16 +70,16 @@ probabilistic:
 
 weights:
   goal_weight: 10.
-  velocity: 0.3 
+  velocity: 0.1 
   acceleration: 0.25 #0.15
-  angular_velocity: 0.5 #0.25 
+  angular_velocity: 0.1 #0.25 
   reference_velocity: 1.5 #2.0 
-  contour: 0.05 #0.01 #0.05
+  contour: 0.01 #0.01 #0.05
   preview: 0.0
   lag: 0.1
   slack: 10000.
-  terminal_angle: 100.0 # 0.0
-  terminal_contouring: 10.0 # 0.0
+  terminal_angle: 0.0 #100.0 # 0.0
+  terminal_contouring: 0.0 #10.0 # 0.0
 
 visualization:
   draw_every: 5 # stages

mpc_planner_jackal/include/mpc_planner_jackal/ros1_jackal.h

@@ -70,8 +70,8 @@ class JackalPlanner
 
     double _measured_velocity{0.};
 
-    double y_max{1.6}; // 2.6 when the blocks are not at the wall
-    double y_min{-1.6};
+    double y_max{2.4}; // 2.6 when the blocks are not at the wall
+    double y_min{-2.0};
     double x_max{3.5};
     double x_min{-3.5};
 

mpc_planner_jackal/launch/ros1_jackal.launch

@@ -28,11 +28,13 @@
   </include> -->
 
   <!-- Start the vicon bridge -->
-<include file="$(find vicon_bridge)/launch/vicon.launch">
+  <include file="$(find vicon_bridge)/launch/vicon.launch">
       <arg name="object_names" value="[$(arg jackal_name), dynamic_object1, dynamic_object2, dynamic_object3, dynamic_object4, dynamic_object5, dingo1]"/>
+      <!-- <arg name="object_names" value="[$(arg jackal_name), dynamic_object1, dynamic_object2, dynamic_object3, dynamic_object5, dingo1, dingo2]"/> -->
       <arg name="object_msg_types" default="[geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/PoseWithCovarianceStamped]"/>
       <arg name="object_frame_ids" default="[map, map, map, map, map, map, map]"/>
       <arg name="object_publish_topics" default="[/vicon/$(arg jackal_name), /vicon/dynamic_object1, /vicon/dynamic_object2, /vicon/dynamic_object3, /vicon/dynamic_object4, /vicon/dynamic_object5, /vicon/dingo1]"/>
+      <!-- <arg name="object_publish_topics" default="[/vicon/$(arg jackal_name), /vicon/dynamic_object1, /vicon/dynamic_object2, /vicon/dynamic_object3, /vicon/dynamic_object5, /vicon/dingo1, /vicon/dingo2]"/> -->
       <arg name="object_frequency_divider" default="[2, 2, 2, 2, 2, 2, 2]"/>
   </include>
 
@@ -43,6 +45,7 @@
     <arg name="robot_topic" value="$(arg jackal_name)"/>
     <!-- 5 Objects -->
     <arg name="dynamic_object_topics" default="[$(arg jackal_name), dynamic_object1, dynamic_object2, dynamic_object3, dynamic_object4, dynamic_object5]"/>
+    <!-- <arg name="dynamic_object_topics" default="[$(arg jackal_name), dynamic_object1, dynamic_object2, dynamic_object3, dingo1, dingo2]"/> -->
     <!-- 2 Objects -->
     <!-- <arg name="dynamic_object_topics" default="[$(arg jackal_name), dynamic_object1, dynamic_object2]"/>  -->
     <arg name="dynamic_object_radius" default="0.4"/>

mpc_planner_jackal/rviz/ros1.rviz

@@ -87,31 +87,16 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
-        VLP16:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        VLP16_base_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
         base_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        baseplate_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
         chassis_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        fenders_link:
+        front_fender_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
@@ -130,15 +115,6 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        hokuyo_base_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        hokuyo_base_scan:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
         imu_link:
           Alpha: 1
           Show Axes: false
@@ -152,6 +128,11 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        rear_fender_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         rear_left_wheel_link:
           Alpha: 1
           Show Axes: false
@@ -175,13 +156,13 @@ Visualization Manager:
     - Class: rviz/Group
       Displays:
         - Class: rviz/MarkerArray
-          Enabled: true
+          Enabled: false
           Marker Topic: /jackal_planner/planned_trajectory
           Name: Planned Trajectory
           Namespaces:
-            "": true
+            {}
           Queue Size: 100
-          Value: true
+          Value: false
         - Class: rviz/MarkerArray
           Enabled: true
           Marker Topic: /jackal_planner/contouring/path
@@ -217,21 +198,21 @@ Visualization Manager:
         - Class: rviz/Group
           Displays:
             - Class: rviz/MarkerArray
-              Enabled: true
+              Enabled: false
               Marker Topic: /jackal_planner/guidance_planner/start_and_goals
               Name: Start and Goals
               Namespaces:
-                "": true
+                {}
               Queue Size: 100
-              Value: true
+              Value: false
             - Class: rviz/MarkerArray
-              Enabled: true
+              Enabled: false
               Marker Topic: /jackal_planner/guidance_planner/graph
               Name: Graph
               Namespaces:
-                "": true
+                {}
               Queue Size: 100
-              Value: true
+              Value: false
             - Class: rviz/MarkerArray
               Enabled: false
               Marker Topic: /lmpcc/homotopy/geometric_paths
@@ -391,7 +372,7 @@ Visualization Manager:
           Scale: 1
           Value: true
         Value: false
-      Enabled: true
+      Enabled: false
       Keep: 100
       Name: Past Trajectory
       Position Tolerance: 0.10000000149011612
@@ -408,23 +389,23 @@ Visualization Manager:
         Value: Arrow
       Topic: /odometry/filtered
       Unreliable: false
-      Value: true
+      Value: false
     - Class: rviz/MarkerArray
-      Enabled: true
+      Enabled: false
       Marker Topic: /jackal_planner/robot_area
       Name: Robot Area
       Namespaces:
-        "": true
+        {}
       Queue Size: 100
-      Value: true
+      Value: false
     - Class: rviz/MarkerArray
-      Enabled: true
+      Enabled: false
       Marker Topic: /jackal_planner/goal
       Name: Goals
       Namespaces:
-        "": true
+        {}
       Queue Size: 100
-      Value: true
+      Value: false
     - Class: rviz/MarkerArray
       Enabled: true
       Marker Topic: /jackal_planner/goal_module
@@ -479,15 +460,15 @@ Visualization Manager:
       Near Clip Distance: 0.009999999776482582
       Pitch: 0.8647974729537964
       Target Frame: <Fixed Frame>
-      Yaw: 3.101998805999756
+      Yaw: 3.140000104904175
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
   Height: 1016
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd0000000400000000000001df0000035efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000011100000375fc0200000003fb0000000a00560069006500770073000000003d00000375000000a000fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000006b8000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000005530000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001df0000035efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001110000035efc0200000003fb0000000a00560069006500770073000000003b0000035e000000a000fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000006b8000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000005530000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:

mpc_planner_jackal/scripts/generate_jackal_solver.py

@@ -58,8 +58,14 @@ def configuration_tmpc(settings):
     base_module.weigh_variable(var_name="a", weight_names="acceleration")
     base_module.weigh_variable(var_name="w", weight_names="angular_velocity")
 
+    base_module.weigh_variable(
+        var_name="v",
+        weight_names=["velocity", "reference_velocity"],
+        cost_function=lambda x, w: w[0] * (x - w[1]) ** 2,
+    )
+
     modules.add_module(ContouringModule(settings))
-    modules.add_module(PathReferenceVelocityModule(settings))
+    # modules.add_module(PathReferenceVelocityModule(settings))
 
     # modules.add_module(GuidanceConstraintModule(settings, constraint_submodule=EllipsoidConstraintModule))
     modules.add_module(GuidanceConstraintModule(settings, constraint_submodule=GaussianConstraintModule))
@@ -75,9 +81,14 @@ def configuration_lmpcc(settings):
     base_module = modules.add_module(MPCBaseModule(settings))
     base_module.weigh_variable(var_name="a", weight_names="acceleration")
     base_module.weigh_variable(var_name="w", weight_names="angular_velocity")
+    base_module.weigh_variable(
+        var_name="v",
+        weight_names=["velocity", "reference_velocity"],
+        cost_function=lambda x, w: w[0] * (x - w[1]) ** 2,
+    )
 
     modules.add_module(ContouringModule(settings))
-    modules.add_module(PathReferenceVelocityModule(settings))
+    # modules.add_module(PathReferenceVelocityModule(settings))
 
     modules.add_module(EllipsoidConstraintModule(settings))
 
@@ -86,6 +97,7 @@ def configuration_lmpcc(settings):
 
 settings = load_settings()
 
+# model, modules = configuration_lmpcc(settings)
 model, modules = configuration_tmpc(settings)
 
 generate_solver(modules, model, settings)

mpc_planner_modules/include/mpc_planner_modules/gaussian_constraints.h

@@ -19,6 +19,7 @@ namespace MPCPlanner
     void visualize(const RealTimeData &data, const ModuleData &module_data) override;
 
   private:
+    double _dummy_x{0.}, _dummy_y{0.};
   };
 }
 #endif // __GAUSSIAN_CONSTRAINTS_H_

mpc_planner_modules/src/gaussian_constraints.cpp

@@ -24,6 +24,8 @@ namespace MPCPlanner
     (void)state;
     (void)data;
     (void)module_data;
+    _dummy_x = state.get("x") + 100.;
+    _dummy_y = state.get("y") + 100.;
   }
 
   void GaussianConstraints::setParameters(const RealTimeData &data, const ModuleData &module_data, int k)
@@ -34,6 +36,21 @@ namespace MPCPlanner
     for (int d = 0; d < CONFIG["n_discs"].as<int>(); d++)
       setSolverParameterEgoDiscOffset(k, _solver->_params, data.robot_area[d].offset, d);
 
+    if (k == 0) // Dummies
+    {
+
+      for (size_t i = 0; i < data.dynamic_obstacles.size(); i++)
+      {
+        setSolverParameterGaussianObstX(k, _solver->_params, _dummy_x, i);
+        setSolverParameterGaussianObstY(k, _solver->_params, _dummy_y, i);
+        setSolverParameterGaussianObstMajor(k, _solver->_params, 0.1, i);
+        setSolverParameterGaussianObstMinor(k, _solver->_params, 0.1, i);
+        setSolverParameterGaussianObstRisk(k, _solver->_params, 0.05, i);
+        setSolverParameterGaussianObstR(k, _solver->_params, 0.1, i);
+      }
+      return;
+    }
+
     std::vector<DynamicObstacle> copied_obstacles = data.dynamic_obstacles;
 
     for (size_t i = 0; i < copied_obstacles.size(); i++)