diff --git a/mep3_behavior/include/mep3_behavior/bt_action_node.hpp b/mep3_behavior/include/mep3_behavior/bt_action_node.hpp index c86ec17f5..f3e338adf 100644 --- a/mep3_behavior/include/mep3_behavior/bt_action_node.hpp +++ b/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 node_; std::string action_name_; @@ -335,14 +336,37 @@ template 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 inline + bool RosActionNode::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 diff --git a/mep3_behavior/strategies/big_strategy_demo.xml b/mep3_behavior/strategies/big_strategy_demo.xml index 8607e3beb..2169e5df9 100644 --- a/mep3_behavior/strategies/big_strategy_demo.xml +++ b/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"/> diff --git a/mep3_navigation/params/nav2_params_big.yaml b/mep3_navigation/params/nav2_params_big.yaml index 8e371511b..84fe5ae6f 100644 --- a/mep3_navigation/params/nav2_params_big.yaml +++ b/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 diff --git a/mep3_navigation/params/nav2_params_small.yaml b/mep3_navigation/params/nav2_params_small.yaml index 10d304753..4746a835a 100644 --- a/mep3_navigation/params/nav2_params_small.yaml +++ b/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 diff --git a/mep3_navigation/src/dilation_layer/dilation_layer.cpp b/mep3_navigation/src/dilation_layer/dilation_layer.cpp index c38826a56..57fc357cb 100644 --- a/mep3_navigation/src/dilation_layer/dilation_layer.cpp +++ b/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_); } diff --git a/mep3_simulation/launch/simulation_launch.py b/mep3_simulation/launch/simulation_launch.py index c4a2b1da0..3e54c4fb1 100644 --- a/mep3_simulation/launch/simulation_launch.py +++ b/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'), diff --git a/mep3_simulation/resource/big_controllers.yaml b/mep3_simulation/resource/big_controllers.yaml new file mode 100644 index 000000000..860b2351f --- /dev/null +++ b/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 diff --git a/mep3_simulation/resource/small_controllers.yaml b/mep3_simulation/resource/small_controllers.yaml new file mode 100644 index 000000000..cb4ed1217 --- /dev/null +++ b/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