From 9d909db16016c344de71c67caeb0d136013eb9e1 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Wed, 30 Aug 2023 09:24:50 +0200 Subject: [PATCH] changes --- .../sm_dancebot_mine_ue.hpp | 7 +- ...x.hpp => st_navigate_mine_waypoints_x.hpp} | 12 +- .../st_navigate_reverse_mine_waypoints_x.hpp | 63 +++++ .../states/st_turn_around.hpp | 58 +++++ .../launch/sm_dancebot_mine_ue_launch.py | 4 +- .../backwards_waypoints_mine.yaml | 232 ++++++++++++++++++ .../params/nav2z_client/waypoints_mine.yaml | 4 +- .../params/sm_dancebot_mine_ue_config.yaml | 1 + 8 files changed, 367 insertions(+), 14 deletions(-) rename smacc2_sm_reference_library/sm_dancebot_mine_ue/include/sm_dancebot_mine_ue/states/{st_navigate_mine_waypoints.x.hpp => st_navigate_mine_waypoints_x.hpp} (70%) create mode 100644 smacc2_sm_reference_library/sm_dancebot_mine_ue/include/sm_dancebot_mine_ue/states/st_navigate_reverse_mine_waypoints_x.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_mine_ue/include/sm_dancebot_mine_ue/states/st_turn_around.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_mine_ue/params/nav2z_client/backwards_waypoints_mine.yaml diff --git a/smacc2_sm_reference_library/sm_dancebot_mine_ue/include/sm_dancebot_mine_ue/sm_dancebot_mine_ue.hpp b/smacc2_sm_reference_library/sm_dancebot_mine_ue/include/sm_dancebot_mine_ue/sm_dancebot_mine_ue.hpp index ec6e88032..68363eeae 100644 --- a/smacc2_sm_reference_library/sm_dancebot_mine_ue/include/sm_dancebot_mine_ue/sm_dancebot_mine_ue.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_mine_ue/include/sm_dancebot_mine_ue/sm_dancebot_mine_ue.hpp @@ -60,6 +60,8 @@ namespace sm_dancebot_mine_ue //STATE FORWARD DECLARATIONS class StAcquireSensors; class StNavigateMineWaypointsX; +class StNavigateReverseMineWaypointsX; +class StTurnAround; class StFinalState; //SUPERSTATE FORWARD DECLARATIONS @@ -137,7 +139,10 @@ struct SmDanceBot : public smacc2::SmaccStateMachineBase //STATES -#include +#include +#include +#include + #include diff --git a/smacc2_sm_reference_library/sm_dancebot_mine_ue/include/sm_dancebot_mine_ue/states/st_navigate_mine_waypoints.x.hpp b/smacc2_sm_reference_library/sm_dancebot_mine_ue/include/sm_dancebot_mine_ue/states/st_navigate_mine_waypoints_x.hpp similarity index 70% rename from smacc2_sm_reference_library/sm_dancebot_mine_ue/include/sm_dancebot_mine_ue/states/st_navigate_mine_waypoints.x.hpp rename to smacc2_sm_reference_library/sm_dancebot_mine_ue/include/sm_dancebot_mine_ue/states/st_navigate_mine_waypoints_x.hpp index 0de69e2a2..41b6fafa1 100644 --- a/smacc2_sm_reference_library/sm_dancebot_mine_ue/include/sm_dancebot_mine_ue/states/st_navigate_mine_waypoints.x.hpp +++ b/smacc2_sm_reference_library/sm_dancebot_mine_ue/include/sm_dancebot_mine_ue/states/st_navigate_mine_waypoints_x.hpp @@ -40,15 +40,9 @@ struct StNavigateMineWaypointsX : smacc2::SmaccState, + Transition, Transition, StNavigateMineWaypointsX, TRANSITION_1> - // Transition, SS5::SsSPattern1, SUCCESS>, - // Transition, SS4::SsFPattern1, SUCCESS>, - // Transition, SS1::SsRadialPattern1, SUCCESS>, - // Transition, StNavigateMineWaypointsX, SUCCESS>, - // Transition, StNavigateMineWaypointsX, ABORT>, - // Transition, StNavigateMineWaypointsX, ABORT> - // // Transition, SS2::SsRadialPattern2, TRANSITION_4> + >reactions; // STATE FUNCTIONS @@ -57,7 +51,7 @@ struct StNavigateMineWaypointsX : smacc2::SmaccState(); // configure_orthogonal(); - configure_orthogonal("waypoints_mine", "sm_dancebot_mine_ue"); + // configure_orthogonal("waypoints_mine", "sm_dancebot_mine_ue"); configure_orthogonal(); } diff --git a/smacc2_sm_reference_library/sm_dancebot_mine_ue/include/sm_dancebot_mine_ue/states/st_navigate_reverse_mine_waypoints_x.hpp b/smacc2_sm_reference_library/sm_dancebot_mine_ue/include/sm_dancebot_mine_ue/states/st_navigate_reverse_mine_waypoints_x.hpp new file mode 100644 index 000000000..b140802fa --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_mine_ue/include/sm_dancebot_mine_ue/states/st_navigate_reverse_mine_waypoints_x.hpp @@ -0,0 +1,63 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include + +namespace sm_dancebot_mine_ue +{ + +// STATE DECLARATION +struct StNavigateReverseMineWaypointsX : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // CUSTOM TRANSITION TAGS + struct TRANSITION_1 : SUCCESS{}; + struct TRANSITION_2 : SUCCESS{}; + struct TRANSITION_3 : SUCCESS{}; + struct TRANSITION_4 : SUCCESS{}; + struct TRANSITION_5 : SUCCESS{}; + struct TRANSITION_6 : SUCCESS{}; + + // TRANSITION TABLE + typedef mpl::list< + Transition, + Transition, StNavigateReverseMineWaypointsX, TRANSITION_1> + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(); + } + + void onEntry() + { + } + + void runtimeConfigure() {} + + void onExit(ABORT) + { + } +}; +} // namespace sm_dancebot_mine_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_mine_ue/include/sm_dancebot_mine_ue/states/st_turn_around.hpp b/smacc2_sm_reference_library/sm_dancebot_mine_ue/include/sm_dancebot_mine_ue/states/st_turn_around.hpp new file mode 100644 index 000000000..0d990f7a8 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_mine_ue/include/sm_dancebot_mine_ue/states/st_turn_around.hpp @@ -0,0 +1,58 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include + +namespace sm_dancebot_mine_ue +{ +using namespace smacc2::default_events; +using smacc2::client_behaviors::CbSleepFor; +using namespace std::chrono_literals; + +// STATE DECLARATION +struct StTurnAround : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + // Transition, StNavigateReverseMineWaypointsX, SUCCESS> + Transition, StNavigateReverseMineWaypointsX, SUCCESS> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + // configure_orthogonal(M_PI, 0.8); + configure_orthogonal(2s); + configure_orthogonal("waypoints_mine_reverse", "sm_dancebot_mine_ue"); + } + + void runtimeConfigure() + { + // auto spinningBehavior = this->getOrthogonal()->getClientBehavior(); + // spinningBehavior->yaw_goal_tolerance_rads_ = 0.2; + + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_mine_ue/launch/sm_dancebot_mine_ue_launch.py b/smacc2_sm_reference_library/sm_dancebot_mine_ue/launch/sm_dancebot_mine_ue_launch.py index f26e5c69f..308a7d5c2 100644 --- a/smacc2_sm_reference_library/sm_dancebot_mine_ue/launch/sm_dancebot_mine_ue_launch.py +++ b/smacc2_sm_reference_library/sm_dancebot_mine_ue/launch/sm_dancebot_mine_ue_launch.py @@ -227,7 +227,7 @@ def generate_launch_description(): ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_gazebo_headless_cmd) ld.add_action(declare_params_file_cmd) - ld.add_action(declare_bt_xml_cmd) + # ld.add_action(declare_bt_xml_cmd) # ld.add_action(declare_autostart_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_show_gz_lidar) @@ -243,6 +243,6 @@ def generate_launch_description(): # # Add the actions to launch all of the navigation nodes ld.add_action(start_robot_state_publisher_cmd) ld.add_action(rviz_cmd) - ld.add_action(bringup_cmd) + # ld.add_action(bringup_cmd) return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_mine_ue/params/nav2z_client/backwards_waypoints_mine.yaml b/smacc2_sm_reference_library/sm_dancebot_mine_ue/params/nav2z_client/backwards_waypoints_mine.yaml new file mode 100644 index 000000000..e2a4018fb --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_mine_ue/params/nav2z_client/backwards_waypoints_mine.yaml @@ -0,0 +1,232 @@ +waypoints: + + # POINT 23 + - position: + x: 85.20 + y: -23.45 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT 22 + - position: + x: 81.00 + y: -26.15 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT 21 + - position: + x: 76.70 + y: -28.15 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT 20 + - position: + x: 72.70 + y: -29.05 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT 19 + - position: + x: 51.70 + y: -32.45 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT 18 + - position: + x: 44.90 + y: -33.65 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT 17 + - position: + x: 41.00 + y: -33.85 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT 16 + - position: + x: 31.00 + y: -33.35 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT 15 + - position: + x: 27.50 + y: -33.75 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT 14 + - position: + x: 21.70 + y: -34.25 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT 13 + - position: + x: 15.20 + y: -32.95 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT 12 + - position: + x: 7.40 + y: -29.65 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT 11 + - position: + x: 4.30 + y: -26.65 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT 10 + - position: + x: 0.43 + y: -26.65 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT 9 + - position: + x: 0.80 + y: -21.05 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT 8 + - position: + x: -0.99 + y: -15.65 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT 7 + - position: + x: -2.79 + y: -11.75 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT 6 + - position: + x: -4.59 + y: -5.35 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT 5 + - position: + x: -4.59 + y: 0.24 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT 4 + - position: + x: -4.99 + y: 6.04 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT 3 + - position: + x: -5.49 + y: 12.74 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT 2 + - position: + x: -5.19 + y: 29.14 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT 1 + - position: + x: -4.29 + y: 53.44 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 \ No newline at end of file diff --git a/smacc2_sm_reference_library/sm_dancebot_mine_ue/params/nav2z_client/waypoints_mine.yaml b/smacc2_sm_reference_library/sm_dancebot_mine_ue/params/nav2z_client/waypoints_mine.yaml index def975f0b..c82b25797 100644 --- a/smacc2_sm_reference_library/sm_dancebot_mine_ue/params/nav2z_client/waypoints_mine.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_mine_ue/params/nav2z_client/waypoints_mine.yaml @@ -101,8 +101,8 @@ waypoints: w: 1.0 # POINT 10 - position: - x: -0.08 - y: -0.43 + x: 0.43 + y: -26.65 z: 0.0 orientation: x: 0.0 diff --git a/smacc2_sm_reference_library/sm_dancebot_mine_ue/params/sm_dancebot_mine_ue_config.yaml b/smacc2_sm_reference_library/sm_dancebot_mine_ue/params/sm_dancebot_mine_ue_config.yaml index b4f36807c..db09f2074 100644 --- a/smacc2_sm_reference_library/sm_dancebot_mine_ue/params/sm_dancebot_mine_ue_config.yaml +++ b/smacc2_sm_reference_library/sm_dancebot_mine_ue/params/sm_dancebot_mine_ue_config.yaml @@ -8,3 +8,4 @@ SmDanceBotMineUE: record_point_distance_threshold: 0.1 waypoints_mine: $(pkg_share)/params/nav2z_client/waypoints_mine.yaml + waypoints_mine_reverse: $(pkg_share)/params/nav2z_client/backwards_waypoints_mine.yaml