Merging dev/social_nav into dev/gap_prop for benchmarking dynamic gap with social cost

include/dynamic_gap/Planner.h

@@ -438,6 +438,14 @@ namespace dynamic_gap
             ros::Publisher mpcInputPublisher_; /**< ROS publisher for mpc input terms */
             ros::Subscriber mpcOutputSubscriber_; /**< ROS subscriber for mpc output */
 
+            ros::Publisher pnTrajPub_; /**< for visualizing pn traj */
+            ros::Publisher pnCand0Pub_;
+            ros::Publisher pnCand1Pub_;
+            ros::Publisher pnCand2Pub_;
+            ros::Publisher bestPnTrajPub_;
+
+
+
             ros::Subscriber tfSub_; /**< Subscriber to TF tree */
             ros::Subscriber laserSub_; /**< Subscriber to incoming laser scan */
 

include/dynamic_gap/config/DynamicGapConfig.h

@@ -66,6 +66,8 @@ namespace dynamic_gap
                 bool projection_operator = true; /**< Boolean for if planner should apply projection operator */
                 bool gap_feasibility_check = true; /**< Flag for enacting gap feasibility checking */
                 bool perfect_gap_models = false; /**< Flag for using perfect gap models */
+                bool social_cost_function = true; /**< false - old without cost function, true - new */
+                float social_cost_weight = .05; /**< weight for social cost during pose evaluation for traj selection */
             } planning;            
 
             /**

include/dynamic_gap/trajectory_evaluation/TrajectoryEvaluator.h

@@ -3,6 +3,7 @@
 #include <ros/ros.h>
 #include <math.h>
 #include <dynamic_gap/utils/Gap.h>
+#include <dynamic_gap/utils/Ungap.h>
 #include <dynamic_gap/utils/Trajectory.h>
 #include <dynamic_gap/config/DynamicGapConfig.h>
 #include <vector>
@@ -56,7 +57,20 @@ namespace dynamic_gap
                                     std::vector<float> & posewiseCosts,
                                     float & terminalPoseCost,
                                     const std::vector<sensor_msgs::LaserScan> & futureScans,
-                                    const int & scanIdx);
+                                    const int & scanIdx,
+                                    dynamic_gap::Gap* gap = nullptr);
+
+            void evaluateUngapTrajectory(const Trajectory & traj,
+                std::vector<float> & posewiseCosts,
+                float & terminalPoseCost,
+                const std::vector<sensor_msgs::LaserScan> & futureScans,
+                const int & scanIdx,
+                dynamic_gap::Ungap* ungap = nullptr); 
+
+            float relativeVelocityCost(Eigen::Vector2f relativeVel,
+                Eigen::Vector2f relativeGapPos,
+                Eigen::Vector2f trajPos,
+                Eigen::Vector2f robotVel);
 
         private:
             /**

include/dynamic_gap/trajectory_generation/GapTrajectoryGenerator.h

@@ -44,10 +44,17 @@ namespace dynamic_gap
             * \param runGoToGoal boolean for if go to goal trajectory method should be run
             * \return trajectory through gap
             */
+
             Trajectory generateTrajectoryV2(Gap * selectedGap, 
                                             const geometry_msgs::PoseStamped & currPose, 
                                             // const geometry_msgs::TwistStamped & currVel,
                                             const geometry_msgs::PoseStamped & globalGoalRobotFrame);
+
+
+            std::vector<Trajectory> generateMultiTrajectoryV2(Gap * selectedGap, 
+                                            const geometry_msgs::PoseStamped & currPose, 
+                                            // const geometry_msgs::TwistStamped & currVel,
+                                            const geometry_msgs::PoseStamped & globalGoalRobotFrame);
 
             // /**
             // * \brief generate local collision-free trajectory through gap

include/dynamic_gap/utils/UngapPoint.h

@@ -47,7 +47,12 @@ namespace dynamic_gap
             Estimator * getModel() const
             {
                 return model_;
-            }            
+            }
+
+            int getUngapID() const
+            {
+                return ungapID_;
+            }
 
         private:
 

rviz/arena.rviz

@@ -170,7 +170,7 @@ Visualization Manager:
       Color map offset: 0
       Color transform: Constant color
       Delete after no. cycles: 100
-      Enabled: false
+      Enabled: true
       Excluded person IDs: ""
       Font color: 255; 255; 255
       Font color style: Same color
@@ -204,7 +204,7 @@ Visualization Manager:
         Value: Bounding boxes
       Topic: /pedsim_visualizer/tracked_persons
       Unreliable: false
-      Value: false
+      Value: true
       Z offset:
         Use Z position from message: false
         Value: 0
@@ -332,13 +332,13 @@ Visualization Manager:
         - Class: rviz/Group
           Displays:
             - Class: rviz/Marker
-              Enabled: true
+              Enabled: false
               Marker Topic: /rto/raw_gaps
               Name: Marker
               Namespaces:
-                raw: true
+                {}
               Queue Size: 100
-              Value: true
+              Value: false
             - Class: rviz/Marker
               Enabled: false
               Marker Topic: /rto/simp_gaps
@@ -348,21 +348,21 @@ Visualization Manager:
               Queue Size: 100
               Value: false
             - Class: rviz/Marker
-              Enabled: false
+              Enabled: true
               Marker Topic: /rto/manip_gaps
               Name: Marker
               Namespaces:
                 {}
               Queue Size: 100
-              Value: false
+              Value: true
             - Class: rviz/MarkerArray
-              Enabled: true
+              Enabled: false
               Marker Topic: /rto/raw_gap_model_positions
               Name: MarkerArray
               Namespaces:
-                raw: true
+                {}
               Queue Size: 100
-              Value: true
+              Value: false
             - Class: rviz/MarkerArray
               Enabled: false
               Marker Topic: /rto/raw_gap_model_velocities
@@ -388,19 +388,19 @@ Visualization Manager:
               Queue Size: 100
               Value: false
             - Class: rviz/MarkerArray
-              Enabled: false
+              Enabled: true
               Marker Topic: /rto/manip_gap_model_positions
               Name: MarkerArray
               Namespaces:
-                {}
+                manip: true
               Queue Size: 100
-              Value: false
+              Value: true
             - Class: rviz/MarkerArray
-              Enabled: false
+              Enabled: true
               Marker Topic: /rto/manip_gap_model_velocities
               Name: MarkerArray
               Namespaces:
-                {}
+                manip: true
               Queue Size: 100
               Value: false
             - Class: rviz/Marker
@@ -412,13 +412,13 @@ Visualization Manager:
               Queue Size: 100
               Value: false
             - Class: rviz/MarkerArray
-              Enabled: false
+              Enabled: true
               Marker Topic: /rto/gap_goal_positions
               Name: MarkerArray
               Namespaces:
-                {}
+                gap_goal: true
               Queue Size: 100
-              Value: false
+              Value: true
             - Class: rviz/MarkerArray
               Enabled: false
               Marker Topic: /rto/gap_goal_velocities
@@ -428,13 +428,13 @@ Visualization Manager:
               Queue Size: 100
               Value: false
             - Class: rviz/MarkerArray
-              Enabled: false
+              Enabled: true
               Marker Topic: /rto/gap_tubes
               Name: MarkerArray
               Namespaces:
-                {}
+                manip_initial: true
               Queue Size: 100
-              Value: false
+              Value: true
             - Class: rviz/MarkerArray
               Enabled: false
               Marker Topic: /rto/propagated_gap_points
@@ -480,21 +480,21 @@ Visualization Manager:
               Queue Size: 100
               Value: false
             - Class: rviz/MarkerArray
-              Enabled: false
+              Enabled: true
               Marker Topic: /rto/candidate_ungap_trajectories
               Name: MarkerArray
               Namespaces:
-                {}
+                allTraj: true
               Queue Size: 100
-              Value: false
+              Value: true
             - Class: rviz/MarkerArray
-              Enabled: false
+              Enabled: true
               Marker Topic: /rto/candidate_tube_trajectories
               Name: MarkerArray
               Namespaces:
-                {}
+                allTraj: true
               Queue Size: 100
-              Value: false
+              Value: true
           Enabled: true
           Name: Trajectories
         - Class: rviz/Group
@@ -527,21 +527,21 @@ Visualization Manager:
           Enabled: true
           Name: Tracking
         - Class: rviz/Marker
-          Enabled: false
+          Enabled: true
           Marker Topic: /rto/planning_loop_idx
           Name: Marker
           Namespaces:
-            {}
+            planning_loop_idx: true
           Queue Size: 100
-          Value: false
+          Value: true
         - Class: rviz/MarkerArray
-          Enabled: true
+          Enabled: false
           Marker Topic: /rto/pedestrian_histories
           Name: MarkerArray
           Namespaces:
             {}
           Queue Size: 100
-          Value: true
+          Value: false
       Enabled: true
       Name: Dynamic Gap
     - Alpha: 1
@@ -749,7 +749,7 @@ Visualization Manager:
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
-      Value: true
+      Value: false
     - Alpha: 1
       Axes Length: 1
       Axes Radius: 0.10000000149011612
@@ -811,18 +811,18 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Scale: 88.0296630859375
+      Scale: 77.46610260009766
       Target Frame: rto/base_link
-      X: -0.21646548807621002
-      Y: -0.13326548039913177
+      X: 1.2442842721939087
+      Y: 0.2725197970867157
     Saved: ~
 Window Geometry:
   Displays:
-    collapsed: true
+    collapsed: false
   Height: 1016
-  Hide Left Dock: true
+  Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd00000004000000000000022d0000039efc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000198000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000039e000000c900ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000041800fffffffb0000000800540069006d00650100000000000004500000000000000000000007380000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000022d0000039efc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000198000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000041800fffffffb0000000800540069006d00650100000000000004500000000000000000000005050000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time: