Changed behavior tree actions to ignore costmap

toid_behaviors/include/toid_behaviors/move_coords.hpp

@@ -47,6 +47,7 @@ protected:
   double target_angle_;
   double target_sign_;
   bool backwards_;
+  unsigned char mode_;
 
   //State
   double last_speed_;

toid_behaviors/src/move_coords.cpp

@@ -74,6 +74,8 @@ ResultStatus MoveCoords::onStart(
     node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_);
   }
 
+  mode_ = command->mode;
+
   scl_.k_phi = k_phi_;
   scl_.k_delta = k_delta_;
   scl_.bbeta = beta_;
@@ -94,28 +96,30 @@ double MoveCoords::distanceToTarget(
 {
   const double dx = target_point.x - pose.position.x;
   const double dy = target_point.y - pose.position.y;
-  const double target_sign = backwards? -1.0 : 1.0;
+  const double target_sign = backwards ? -1.0 : 1.0;
 
   return target_sign * (dx * cos(target_theta) + dy * sin(target_theta));
 }
 
-
+double MoveCoords::velocityTarget(const double dist_left)
-double MoveCoords::velocityTarget(const double dist_left) {
+{
   const double lower_bound = last_speed_ - control_duration_ * max_vel_accel_;
   const double upper_bound = last_speed_ + control_duration_ * max_vel_accel_;
 
-
   double vel = max_vel_speed_;
   double max_vel_to_stop = 0.8 * std::sqrt(2.0 * max_vel_decel_ * dist_left);
   vel = std::min(vel, max_vel_to_stop);
   return std::clamp(target_sign_ * vel, lower_bound, upper_bound);
 }
 
-bool MoveCoords::collisionDetection(const geometry_msgs::msg::Pose &pose, geometry_msgs::msg::Pose &last_ok_pose) {
+bool MoveCoords::collisionDetection(
-  const int samples = static_cast<int>(0.5/control_duration_);
+  const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Pose & last_ok_pose)
+{
+  const int samples = static_cast<int>(0.5 / control_duration_);
   geometry_msgs::msg::Pose current_pose = pose;
   last_ok_pose = pose;
-  for(int i = 0; i < samples; i++) {
+  const bool check_map = !(mode_ & MoveAction::Goal::IGNORE_OBSTACLES);
+  for (int i = 0; i < samples; i++) {
     scl_.step(target_pose_, current_pose, control_duration_, backwards_);
 
     geometry_msgs::msg::Pose2D p;
@@ -123,17 +127,18 @@ bool MoveCoords::collisionDetection(const geometry_msgs::msg::Pose &pose, geomet
     p.y = current_pose.position.y;
     p.theta = tf2::getYaw(current_pose.orientation);
 
-    if(!local_collision_checker_->isCollisionFree(p, i==0)) {
+    if (check_map && !local_collision_checker_->isCollisionFree(p, i == 0)) {
       return true;
     }
 
-    if(check_rival_collision(p)) {
+    if (check_rival_collision(p)) {
       return true;
     }
 
     last_ok_pose = current_pose;
-    const double dist_left = distanceToTarget(current_pose, target_pose_.position, target_angle_, backwards_);
+    const double dist_left =
-    if(dist_left < 0.005) {
+      distanceToTarget(current_pose, target_pose_.position, target_angle_, backwards_);
+    if (dist_left < 0.005) {
       return false;
     }
   }
@@ -144,7 +149,6 @@ ResultStatus MoveCoords::updateVel(
   const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &,
   geometry_msgs::msg::Twist & out_vel)
 {
-
   double dist_left = distanceToTarget(pose, target_pose_.position, target_angle_, backwards_);
 
   if (dist_left <= 0.001) {
@@ -157,10 +161,10 @@ ResultStatus MoveCoords::updateVel(
   double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_);
 
   geometry_msgs::msg::Pose last_ok_pose;
-  if(collisionDetection(pose, last_ok_pose)) {
+  if (collisionDetection(pose, last_ok_pose)) {
-
+    dist_left = distanceToTarget(
-    dist_left = distanceToTarget(pose, last_ok_pose.position, tf2::getYaw(last_ok_pose.orientation), backwards_);
+      pose, last_ok_pose.position, tf2::getYaw(last_ok_pose.orientation), backwards_);
-    if(dist_left <= 0.02) {
+    if (dist_left <= 0.02) {
       out_vel.linear.x = 0;
       out_vel.angular.z = 0;
     } else {
@@ -168,8 +172,6 @@ ResultStatus MoveCoords::updateVel(
       scl_.calculate_vel(last_ok_pose, pose, out_vel, backwards_);
     }
 
-
-
     last_speed_ = out_vel.linear.x;
 
     RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left);
@@ -185,11 +187,9 @@ ResultStatus MoveCoords::updateVel(
     return ResultStatus{Status::RUNNING};
   }
 
-
   scl_.v_linear_max = target_sign_ * velocityTarget(dist_left);
   scl_.calculate_vel(target_pose_, pose, out_vel, backwards_);
 
-
   last_speed_ = out_vel.linear.x;
 
   RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left);

toid_bt/include/toid_bt/plugins/move_coords_action.hpp

@@ -53,6 +53,7 @@ public:
 
     goal.max_speed = max_speed;
     goal.backwards = backwards;
+    goal.mode = 1;
 
     return true;
   }

toid_bt/include/toid_bt/plugins/rotate_action.hpp

@@ -40,6 +40,7 @@ public:
     goal.max_speed = max_speed.value();
     goal.angle = angles::from_degrees(goal.angle);
     goal.min_angle = angles::from_degrees(goal.min_angle);
+    goal.mode = 1;
     return true;
   }
 

toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp

@@ -44,6 +44,7 @@ public:
     goal.angle = std::atan2(y - pose.pose.position.y, x - pose.pose.position.x);
     goal.min_angle = angles::from_degrees(min_angle);
     goal.max_speed = max_speed;
+    goal.mode = 1;
 
     return true;
   }