Cutout obstacles outside the terrain

mep3_behavior/include/mep3_behavior/bt_action_node.hpp

@@ -102,6 +102,7 @@ class RosActionNode : public BT::ActionNodeBase
   void cancelGoal();
 
 protected:
+  bool should_cancel_goal();
 
   std::shared_ptr<rclcpp::Node> node_;
   std::string action_name_;
@@ -335,14 +336,37 @@ template<class T> inline
   if (!goal_handle_)
     return;
 
-  auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
+  if (should_cancel_goal()) {
+    auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
 
-  if (rclcpp::spin_until_future_complete(node_, future_cancel, server_timeout_) !=
-      rclcpp::FutureReturnCode::SUCCESS)
+    if (rclcpp::spin_until_future_complete(node_, future_cancel, server_timeout_) !=
+        rclcpp::FutureReturnCode::SUCCESS)
+    {
+      RCLCPP_ERROR( node_->get_logger(),
+                  "Failed to cancel action server for %s", action_name_.c_str());
+    }
+  }
+}
+
+template<class T> inline
+  bool RosActionNode<T>::should_cancel_goal()
+{
+  // Shut the node down if it is currently running
+  if (status() != BT::NodeStatus::RUNNING) {
+    return false;
+  }
+
+  rclcpp::spin_some(node_);
+  auto status = goal_handle_->get_status();
+
+  // Check if the goal is still executing
+  if (status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ||
+    status == action_msgs::msg::GoalStatus::STATUS_EXECUTING)
   {
-    RCLCPP_ERROR( node_->get_logger(),
-                 "Failed to cancel action server for %s", action_name_.c_str());
+    return true;
   }
+
+  return false;
 }
 
 }  // namespace BT

mep3_behavior/strategies/big_strategy_demo.xml

@@ -13,13 +13,13 @@
                      max_velocity="90"
                      position="-90"
                      tolerance="0.3"
-                     feedback_effort="effort"
-                     feedback_position=""
-                     result=""/>
+                     feedback_effort="feedback_effort"
+                     feedback_position="feedback_position"
+                     result="result"/>
       <SetSharedBlackboard output_key="effort"
                            value="{effort}"/>
       <Move goal="0;0;0"
-            ignore_obstacles="true"
+            ignore_obstacles="false"
             max_acceleration="0.5"
             max_velocity="0.8"
             type="0"/>

mep3_navigation/params/nav2_params_big.yaml

@@ -76,12 +76,14 @@
     ros__parameters:
       update_frequency: 20.0
       publish_frequency: 5.0
-      global_frame: odom
+      global_frame: map
       robot_base_frame: base_link
       use_sim_time: false
-      rolling_window: True
+      rolling_window: false
       width: 3
-      height: 3
+      height: 4
+      origin_x: -1.5
+      origin_y: -2.0
       resolution: 0.05
       robot_radius: 0.17
       use_maximum: true
@@ -116,7 +118,11 @@
       global_frame: map
       robot_base_frame: base_link
       use_sim_time: false
-      rolling_window: True
+      rolling_window: false
+      width: 3
+      height: 4
+      origin_x: -1.5
+      origin_y: -2.0
       robot_radius: 0.2
       resolution: 0.07
       use_maximum: true

mep3_navigation/params/nav2_params_small.yaml

@@ -76,12 +76,14 @@
     ros__parameters:
       update_frequency: 20.0
       publish_frequency: 5.0
-      global_frame: odom
+      global_frame: map
       robot_base_frame: base_link
       use_sim_time: false
-      rolling_window: True
+      rolling_window: false
       width: 3
-      height: 3
+      height: 4
+      origin_x: -1.5
+      origin_y: -2.0
       resolution: 0.05
       robot_radius: 0.17
       use_maximum: true
@@ -116,7 +118,11 @@
       global_frame: map
       robot_base_frame: base_link
       use_sim_time: false
-      rolling_window: True
+      rolling_window: false
+      width: 3
+      height: 4
+      origin_x: -1.5
+      origin_y: -2.0
       robot_radius: 0.2
       resolution: 0.07
       use_maximum: true

mep3_navigation/src/dilation_layer/dilation_layer.cpp

@@ -70,6 +70,14 @@ namespace mep3_navigation
     }
 
     cv::Mat mat(master_grid.getSizeInCellsY(), master_grid.getSizeInCellsX(), CV_8UC1, master_grid.getCharMap());
+
+    // TODO: Move to another plugin
+    const int padding = (0.5 + 0.06) / master_grid.getResolution();
+    mat(cv::Rect(0, 0, padding, mat.rows)) = cv::Scalar(0);
+    mat(cv::Rect(mat.cols - padding, 0, padding, mat.rows)) = cv::Scalar(0);
+    mat(cv::Rect(0, 0, mat.cols, padding)) = cv::Scalar(0);
+    mat(cv::Rect(0, mat.rows - padding, mat.cols, padding)) = cv::Scalar(0);
+
     cv::dilate(mat, mat, kernel_);
   }
 

mep3_simulation/launch/simulation_launch.py

@@ -18,6 +18,10 @@ def generate_launch_description():
                              'resource', 'big_controllers.yaml')
     controller_params_file_small = os.path.join(get_package_share_directory('mep3_hardware'),
                              'resource', 'small_controllers.yaml')
+    controller_params_file_big_override = os.path.join(get_package_share_directory('mep3_simulation'),
+                                'resource', 'big_controllers.yaml')
+    controller_params_file_small_override = os.path.join(get_package_share_directory('mep3_simulation'),
+                             'resource', 'small_controllers.yaml')
 
     robot_description_big = pathlib.Path(
         os.path.join(package_dir, 'resource', 'big_description.urdf')).read_text()
@@ -46,7 +50,8 @@ def generate_launch_description():
                 'use_sim_time': True,
                 'robot_description': robot_description_big
             },
-            controller_params_file_big
+            controller_params_file_big,
+            controller_params_file_big_override
         ],
         remappings=[
             ('/big/diffdrive_controller/cmd_vel_unstamped', 'cmd_vel'),
@@ -70,7 +75,8 @@ def generate_launch_description():
                 'use_sim_time': True,
                 'robot_description': robot_description_small
             },
-            controller_params_file_small
+            controller_params_file_small,
+            controller_params_file_small_override
         ],
         remappings=[
             ('/small/diffdrive_controller/cmd_vel_unstamped', 'cmd_vel'),

mep3_simulation/resource/big_controllers.yaml

@@ -0,0 +1,6 @@
+big:
+  diffdrive_controller:
+    ros__parameters:
+      wheel_separation: 0.3
+      wheel_radius: 0.036
+      right_wheel_radius_multiplier: 1.0

mep3_simulation/resource/small_controllers.yaml

@@ -0,0 +1,6 @@
+small:
+  diffdrive_controller:
+    ros__parameters:
+      wheel_separation: 0.3
+      wheel_radius: 0.036
+      right_wheel_radius_multiplier: 1.0