checked the goal reaching

mpc_ros/include/mpc_plannner_ros.h

@@ -138,6 +138,7 @@ namespace mpc_ros{
             */
 
             // Flags
+            std::string odom_topic_;
             bool reached_goal_;
             bool initialized_;
 

mpc_ros/params/mpc_last_params.yaml

@@ -1,3 +1,5 @@
+
+
 # Parameters for control loop
 pub_twist_cmd: true
 debug_info: false
@@ -24,3 +26,46 @@ mpc_max_angvel: 1.0 #2.5 #1.0
 mpc_max_throttle: 1.0 # Maximal throttle accel
 mpc_bound_value: 1.0e3 # Bound value for other variables
 
+MPCPlannerROS:
+# Robot Configuration Parameters
+  odom_topic: "odom"
+  min_trans_vel:  0.01
+
+  max_vel_x: 0.5
+  min_vel_x: -0.05
+
+  max_vel_y: 0.0
+  min_vel_y: 0.0
+
+  # The velocity when robot is moving in a straight line
+  trans_stopped_vel: 0.01
+
+  max_rot_vel: 1.5
+  min_rot_vel: 0.1
+  rot_stopped_vel: 0.01
+
+  acc_lim_x: 2.0
+  acc_lim_theta: 3.0
+  acc_lim_y: 0.0
+
+# Goal Tolerance Parametes
+  yaw_goal_tolerance: 1.0
+  xy_goal_tolerance: 0.5
+  latch_xy_goal_tolerance: false
+
+# Trajectory Scoring Parameters
+  path_distance_bias: 32.0
+  goal_distance_bias: 20.0
+  occdist_scale: 0.02
+  forward_point_distance: 0.325 # when 0 is, off map warn disappear
+  stop_time_buffer: 0.2
+  scaling_speed: 0.25
+  max_scaling_factor: 0.2
+
+# Oscillation Prevention Parameters
+  oscillation_reset_dist: 0.05
+
+# Debugging
+  publish_traj_pc : true
+  publish_cost_grid_pc: true
+ # global_frame_id: odom

mpc_ros/src/mpc_plannner_ros.cpp

@@ -49,6 +49,11 @@ namespace mpc_ros{
 
         planner_util_.initialize(tf, costmap_, costmap_ros_->getGlobalFrameID());
 
+        if( private_nh.getParam( "odom_topic", odom_topic_ ))
+        {
+            odom_helper_.setOdomTopic( odom_topic_ );
+        }
+
         //Private parameters handler
         ros::NodeHandle pn("~");
 
@@ -172,7 +177,7 @@ namespace mpc_ros{
       limits.prune_plan = config.prune_plan;
       limits.trans_stopped_vel = config.trans_stopped_vel;
       limits.theta_stopped_vel = config.theta_stopped_vel;
-      planner_util_.reconfigureCB(limits, true);
+      planner_util_.reconfigureCB(limits, false);
 
   }
 
@@ -267,6 +272,7 @@ namespace mpc_ros{
         }
         ROS_DEBUG_NAMED("mpc_planner", "Received a transformed plan with %zu points.", transformed_plan.size());
         updatePlanAndLocalCosts(current_pose_, transformed_plan, costmap_ros_->getRobotFootprint());
+
         if (latchedStopRotateController_.isPositionReached(&planner_util_, current_pose_)){
             //publish an empty plan because we've reached our goal position
             std::vector<geometry_msgs::PoseStamped> local_plan;