Dương update custom

src/APIs/c_api/src/nav_c_api.cpp

@@ -569,11 +569,16 @@ extern "C" bool navigation_set_twist_linear(NavigationHandle handle,
       return false;
 
     robot_geometry_msgs::Vector3 linear;
-    linear.x = linear_x;
+    linear.x = 0.1;
     linear.y = linear_y;
     linear.z = linear_z;
-    robot::log_info("setTwistLinear %f", linear.x);
+    bool result = nav_ptr->setTwistLinear(linear);
-    return nav_ptr->setTwistLinear(linear);
+    robot::log_info("setTwistLinear Forward %f", linear.x);
+    
+    linear.x = -0.1;
+    result &= result && nav_ptr->setTwistLinear(linear);
+    robot::log_info("setTwistLinear Backward %f", linear.x);
+    return result;
   }
   catch (...)
   {

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp

@@ -404,35 +404,40 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
     }
     
   }
-  else
+  else if(compute_plan_.poses.size() == 1)
   {
     try
     {
-      auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
+      // auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
-      auto prev_carrot_pose_it = transform_plan_.poses.begin();
+      // auto prev_carrot_pose_it = transform_plan_.poses.begin();
-      double distance_it = 0;
+      // double distance_it = 0;
-      for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it)
+      // for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it)
-      {
+      // {
-        double dx = it->pose.x - carrot_pose_it->pose.x;
+      //   double dx = it->pose.x - carrot_pose_it->pose.x;
-        double dy = it->pose.y - carrot_pose_it->pose.y;
+      //   double dy = it->pose.y - carrot_pose_it->pose.y;
-        distance_it += std::hypot(dx, dy);
+      //   distance_it += std::hypot(dx, dy);
-        if (distance_it > costmap_robot_->getCostmap()->getResolution())
+      //   if (distance_it > costmap_robot_->getCostmap()->getResolution())
-        {
+      //   {
-          prev_carrot_pose_it = it;
+      //     prev_carrot_pose_it = it;
-          break;
+      //     break;
-        }
+      //   }
-      }
+      // }
 
-      robot_geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution()
+      // robot_geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution()
-                                      ? robot_nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose)
+      //                                 ? robot_nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose)
-                                      : robot_nav_2d_utils::pose2DToPose(robot_geometry_msgs::Pose2D());
+      //                                 : robot_nav_2d_utils::pose2DToPose(robot_geometry_msgs::Pose2D());
 
-      robot_geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose);
+      // robot_geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose);
 
       // teb_local_planner::PoseSE2 start_pose(front);
       // teb_local_planner::PoseSE2 goal_pose(back);
       // const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec());
-      const double dir_path = 0.0;
+
+      
+      auto goal_pose = compute_plan_.poses.front().pose;
+      auto start_pose = pose.pose;
+      double angle_path = atan2(goal_pose.y - start_pose.y, goal_pose.x - start_pose.x);
+      double dir_path = cos(fabs(angle_path - goal_pose.theta));
       if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9)
         x_direction = dir_path > 0 ? FORWARD : BACKWARD;
     }
@@ -476,8 +481,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     twist = traj_->nextTwist();
   }
   double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
-  // drive_cmd.x = std::min(sqrt(twist.x * twist.x),  fabs(v_max));
+  drive_cmd.x = std::min(sqrt(twist.x * twist.x),  fabs(v_max));
-  drive_cmd.x = sqrt(twist.x * twist.x);
+  // drive_cmd.x = sqrt(twist.x * twist.x);
   
   robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
   if (transformed_plan.poses.empty())
@@ -584,7 +589,11 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   }
 
   if(fabs(v_max == 0.0))
+  {
     drive_cmd.x = 0.0;
+    robot::log_warning_throttle(0.2, "[%s:%d]\n v_max is 0.0", __FILE__, __LINE__);
+    return result;
+  }
   result.velocity = drive_cmd;
   prevous_drive_cmd_ = drive_cmd;
   return result;
@@ -636,26 +645,28 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   double w_target = v_target * kappa;
   if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
   {
-    ss << w_target << " ";
     if (trajectory.poses.size() >= 2) {
       const auto& p1 = trajectory.poses[trajectory.poses.size() - 1].pose;
       double heading_ref = 0.0;
       for(int i = trajectory.poses.size() - 2; i >= 0; i--)
       {
         const auto& p = trajectory.poses[i].pose;
-        const auto& dx = sign_x < 0.0 ? p1.x - p.x : p.x - p1.x;
+        const auto& dx =  p1.x - p.x ;
-        const auto& dy = sign_x < 0.0 ? p1.y - p.y : p.y - p1.y;
+        const auto& dy =  p1.y - p.y ;
         if(std::hypot(dx, dy) >= costmap_robot_->getCostmap()->getResolution())
         {
           if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
             continue;
-          heading_ref = angles::normalize_angle(std::atan2(dy, dx));
+          heading_ref = std::atan2(dy, dx);
+          ss << "error " << heading_ref << " ";
+          if(sign_x < 0.0)
+          heading_ref += std::copysign(M_PI, heading_ref) * (-1.0);
           break;
         }
       }
 
-      const double error = angles::normalize_angle(heading_ref);
+      const double error = heading_ref;
-      ss << "error: " << error << " ";
+      ss << error << " ";
       double w_heading = 0.0;
       pid(error, 
         near_goal_heading_integral_, 
@@ -667,12 +678,13 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
         w_heading);
       // Apply acceleration limits
       double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);
+      ss << "dw_heading " << dw_heading << " ";
       w_target = velocity.theta + dw_heading;
-      ss << w_target << " ";
+      // w_target = std::clamp(w_target, -0.03, 0.03);
     }
     else
     {
-      w_target = 0.0;
+      w_target = std::clamp(w_target, -0.001, 0.001);
       near_goal_heading_was_active_ = false;
     }
   }
@@ -690,7 +702,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   drive_cmd.x = velocity.x + dv;
   drive_cmd.theta = velocity.theta + dw;
 
-  ss  << drive_cmd.theta << " ";
   Eigen::VectorXd y(2);
   y << drive_cmd.x, drive_cmd.theta;
 
@@ -710,7 +721,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
   if (kf_filter_angular_)
     drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
-  ROS_WARN_THROTTLE(0.1, "%s", ss.str().c_str());
+  // robot::log_info("%s", ss.str().c_str());
 }
 
 void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
@@ -768,21 +779,29 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
   // const double max_kappa = calculateMaxKappa(global_plan);
   // const bool curvature = max_kappa > straight_threshold;
   double path_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
-  if(is_stopped && global_plan.poses.size() >= 2)
+  if(is_stopped && global_plan.poses.size() >= 4 &&
+  journey(global_plan.poses, 0, global_plan.poses.size() - 1) >= 0.7 * min_lookahead_dist_)
   {
-    const auto& p1 = global_plan.poses[1];
+    const auto& p1 = global_plan.poses[2];
-    for(int i = 2; i < global_plan.poses.size(); i++)
+    for(int i = 3; i < global_plan.poses.size(); i++)
     {
       const auto& p = global_plan.poses[i];
-      if(std::hypot(p.pose.x, p.pose.y) > costmap_robot_->getCostmap()->getResolution())
+      const auto& dx = p.pose.x - p1.pose.x;
+      const auto& dy = p.pose.y - p1.pose.y;
+      if(std::hypot(dx, dy) > costmap_robot_->getCostmap()->getResolution())
       {
-        path_angle = std::atan2(p.pose.y - p1.pose.y, p.pose.x - p1.pose.x);
+        if(fabs(dx) < 1e-9 && fabs(dy) < 1e-9)
+          continue;
+        path_angle = std::atan2(dy, dx);
         break;
       }
     }
   }
   
   // Whether we should rotate robot to rough path heading
+  // if(sign_x < 0.0)
+  //   path_angle += std::copysign(M_PI, path_angle) * (-1.0);
+  // angle_to_path = path_angle;
   angle_to_path = sign_x < 0.0 ? angles::normalize_angle(M_PI + path_angle) : path_angle;
   double heading_linear = sqrt(velocity.x * velocity.x + velocity.y * velocity.y);
   // The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
@@ -805,6 +824,9 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
   //   ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta);
   //   ROS_INFO_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
   // }
+#else
+  if (result)
+    robot::log_info_throttle(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
 #endif
   return result;
 }
@@ -1241,7 +1263,7 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
 
   if (max_kappa <= straight_threshold && fabs(path.poses.back().pose.x) < min_lookahead_dist_) // nếu đường thẳng
   { 
-    if(fabs(path.poses.front().pose.y) <= 0.03 && fabs(path.poses.back().pose.x) < (min_lookahead_dist_ + max_path_distance_))
+    if(fabs(path.poses.front().pose.y) <= 0.03 && fabs(path.poses.back().pose.x) < min_lookahead_dist_ )
     {
       return generateParallelPath(path, sign_x);
     }

src/Navigations/Packages/move_base/src/move_base.cpp

@@ -976,12 +976,6 @@ bool move_base::MoveBase::moveTo( const robot_geometry_msgs::PoseStamped &goal,
     
     robot::log_info("[MoveBase::moveTo] Processing goal through action server...");
     as_->processGoal(action_goal);
-    
-    setTwistLinear(old_linear_fwd_);
-    setTwistLinear(old_linear_bwd_);
-    setTwistAngular(old_angular_fwd_);
-    setTwistAngular(old_angular_rev_);
-
     robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server");
   }
   catch (const std::exception &e)
@@ -1124,11 +1118,6 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
     }
 
     as_->processGoal(action_goal);
-
-    setTwistLinear(old_linear_fwd_);
-    setTwistLinear(old_linear_bwd_);
-    setTwistAngular(old_angular_fwd_);
-    setTwistAngular(old_angular_rev_);
   }
   catch (const std::exception &e)
   {
@@ -1290,10 +1279,6 @@ bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry
     
     robot::log_info("[MoveBase::moveTo] Processing goal through action server...");
     as_->processGoal(action_goal);
-    setTwistLinear(old_linear_fwd_);
-    setTwistLinear(old_linear_bwd_);
-    setTwistAngular(old_angular_fwd_);
-    setTwistAngular(old_angular_rev_);
     robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server");
   }
   catch (const std::exception &e)
@@ -1440,10 +1425,6 @@ bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
       return false;
     }
     as_->processGoal(action_goal);
-    setTwistLinear(old_linear_fwd_);
-    setTwistLinear(old_linear_bwd_);
-    setTwistAngular(old_angular_fwd_);
-    setTwistAngular(old_angular_rev_);
   }
   catch (const std::exception &e)
   {