Merge pull request #43 from IRobot-Algorithm/navigation-branch

修复第二矿区导航卡顿问题

src/navigation/move_base/launch/navigation.launch

@@ -23,10 +23,10 @@
   <include file="$(find move_base)/launch/move_base.launch"/>
 
   <!-- rviz -->
-  <!-- <group if="$(arg open_rviz)"> 
+  <group if="$(arg open_rviz)"> 
     <node pkg="rviz" type="rviz" name="rviz" required="true"
           args="-d $(find move_base)/rviz/teb.rviz"/>
-  </group> -->
+  </group>
 
 </launch>
 

src/navigation/move_base/param/dwa_local_planner_params.yaml

@@ -34,7 +34,7 @@ DWAPlannerROS:
   # Trajectory Scoring Parameters
   path_distance_bias: 35.0      # 32.0   - weighting for how much it should stick to the global path plan
   # 衡量控制器遵循给定路径的一致程度的加权值
-  goal_distance_bias: 20.0      # 24.0   - wighting for how much it should attempt to reach its goal
+  goal_distance_bias: 24.0      # 24.0   - wighting for how much it should attempt to reach its goal
   # 判断是否接近目标地点和控制速度的加权值
   occdist_scale: 10.0           # 0.01   - weighting for how much the controller should avoid obstacles
   # 有关避障的加权值 

src/navigation/move_base/param/teb_local_planner_params.yaml

@@ -26,13 +26,13 @@ TebLocalPlannerROS:
 #*******************************************************************************
 # Robot
 #*******************************************************************************
-  max_vel_x: 0.5             #最大x前向速度
-  max_vel_x_backwards: 0.5 #最大x后退速度
-  max_vel_y: 0.5             #最大y方向速度
-  max_vel_theta: 0.5        #最大转向角速度
-  acc_lim_x: 3.0            #最大x加速度
-  acc_lim_y: 3.0            #最大x加速度
-  acc_lim_theta: 3.0        #最大角加速度
+  max_vel_x: 1.0             #最大x前向速度
+  max_vel_x_backwards: 1.0 #最大x后退速度
+  max_vel_y: 1.0             #最大y方向速度
+  max_vel_theta: 1.0        #最大转向角速度
+  acc_lim_x: 6.0            #最大x加速度
+  acc_lim_y: 6.0            #最大x加速度
+  acc_lim_theta: 6.0        #最大角加速度
   max_vel_trans: 1.0
   is_footprint_dynamic: False
 
@@ -62,7 +62,7 @@ TebLocalPlannerROS:
 
   # 不是True会震荡
   free_goal_vel: True
-  complete_global_plan: False
+  complete_global_plan: True
 
 #*******************************************************************************
 # Obstacles
@@ -74,7 +74,7 @@ TebLocalPlannerROS:
   obstacle_poses_affected: 10                 #障碍物姿态受影响程度0-30
   costmap_converter_plugin: "point"
   costmap_converter_spin_thread: True
-  costmap_converter_rate: 5
+  costmap_converter_rate: 3
 
   dynamic_obstacle_inflation_dist: 0.0      #动态障碍物的膨胀范围
   include_dynamic_obstacles: False          #是否将动态障碍物预测为速度模型
@@ -87,16 +87,16 @@ TebLocalPlannerROS:
   optimization_verbose: False    #打印优化过程详情
   penalty_epsilon: 0.1           #对于硬约束近似，在惩罚函数中添加安全范围
   obstacle_cost_exponent: 4
-  weight_max_vel_x: 0.2            #最大x速度权重 0~2
-  weight_max_vel_y: 0.2
-  weight_max_vel_theta: 0.2        #最大角速度权重 0~1
+  weight_max_vel_x: 1.0           #最大x速度权重 0~2
+  weight_max_vel_y: 1.0
+  weight_max_vel_theta: 1.0        #最大角速度权重 0~1
   weight_acc_lim_x: 1            #最大x 加速度权重 0~1
   weight_acc_lim_y: 1
   weight_acc_lim_theta: 1        #最大角速度权重 0~1
   weight_kinematics_nh: 0.01     #非完整运动学的优化权重
   weight_kinematics_forward_drive: 0.1     #优化过程中，迫使机器人只选择前进方向，差速轮适用
   weight_kinematics_turning_radius: 0.1    #优化过程中，车型机器人的最小转弯半径的权重
-  weight_optimaltime: 0.6 # must be > 0  #优化过程中，基于轨迹的时间上的权重
+  weight_optimaltime: 1.0 # must be > 0  #优化过程中，基于轨迹的时间上的权重
   weight_shortest_path: 0
   weight_obstacle: 20           #优化过程中，和障碍物最小距离的权重 0~50
   weight_inflation: 0.6          #优化过程中， 膨胀区的权重

src/navigation/teb_local_planner/src/teb_local_planner_ros.cpp

@@ -293,7 +293,8 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt
   double dx = global_goal.pose.position.x - robot_pose_.x();
   double dy = global_goal.pose.position.y - robot_pose_.y();
   double delta_orient = g2o::normalize_theta( tf2::getYaw(global_goal.pose.orientation) - robot_pose_.theta() );
-  if(fabs(std::sqrt(dx*dx+dy*dy)) < cfg_.goal_tolerance.xy_goal_tolerance
+  double dis = fabs(std::sqrt(dx*dx+dy*dy));
+  if(dis < cfg_.goal_tolerance.xy_goal_tolerance
     && fabs(delta_orient) < cfg_.goal_tolerance.yaw_goal_tolerance
     && (!cfg_.goal_tolerance.complete_global_plan || via_points_.size() == 0)
     && (base_local_planner::stopped(base_odom, cfg_.goal_tolerance.theta_stopped_vel, cfg_.goal_tolerance.trans_stopped_vel)
@@ -303,6 +304,18 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt
     return mbf_msgs::ExePathResult::SUCCESS;
   }
 
+  if (dis < 0.3)
+  {
+    Eigen::Vector2d v(cos(robot_pose_.theta()) * dx + sin(robot_pose_.theta()) * dy,
+                    - sin(robot_pose_.theta()) * dx + cos(robot_pose_.theta()) * dy);
+    cmd_vel.twist.linear.x = v(0)/v.norm() * dis * 3;
+    cmd_vel.twist.linear.y = v(1)/v.norm() * dis * 3;
+    cmd_vel.twist.angular.z = delta_orient * 3;
+
+    last_cmd_ = cmd_vel.twist;
+    return mbf_msgs::ExePathResult::SUCCESS;
+  }
+
   // check if we should enter any backup mode and apply settings
   configureBackupModes(transformed_plan, goal_idx);