changes

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<SmDanceBot, MsDanceBotR
 #include <sm_dancebot_mine_ue/superstates/ss_s_pattern_1.hpp>
 
 //STATES
-#include <sm_dancebot_mine_ue/states/st_navigate_mine_waypoints.x.hpp>
+#include <sm_dancebot_mine_ue/states/st_navigate_mine_waypoints_x.hpp>
+#include <sm_dancebot_mine_ue/states/st_turn_around.hpp>
+#include <sm_dancebot_mine_ue/states/st_navigate_reverse_mine_waypoints_x.hpp>
+
 
 #include <sm_dancebot_mine_ue/states/st_acquire_sensors.hpp>
 

smacc2_sm_reference_library/sm_dancebot_mine_ue/include/sm_dancebot_mine_ue/states/st_navigate_mine_waypoints.x.hpp → 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<StNavigateMineWaypointsX, M
 
   // TRANSITION TABLE
   typedef mpl::list<
-    Transition<cl_nav2z::EvWaypointFinal, StFinalState, SUCCESS>,
+    Transition<cl_nav2z::EvWaypointFinal, StTurnAround, SUCCESS>,
     Transition<EvCbSuccess<CbNavigateNextWaypointFree, OrNavigation>, StNavigateMineWaypointsX, TRANSITION_1>
-    // Transition<EvWaypoint3<ClNav2Z, OrNavigation>, SS5::SsSPattern1, SUCCESS>,
-    // Transition<EvWaypoint11<ClNav2Z, OrNavigation>, SS4::SsFPattern1, SUCCESS>,
-    // Transition<EvWaypoint1<ClNav2Z, OrNavigation>, SS1::SsRadialPattern1, SUCCESS>,
-    // Transition<EvCbSuccess<CbNavigateNextWaypoint, OrNavigation>, StNavigateMineWaypointsX, SUCCESS>,
-    // Transition<EvCbFailure<CbNavigateNextWaypoint, OrNavigation>, StNavigateMineWaypointsX, ABORT>, 
-    // Transition<EvActionAborted<ClNav2Z, OrNavigation>, StNavigateMineWaypointsX, ABORT>
-    // // Transition<EvWaypoint2<ClNav2Z, OrNavigation>, SS2::SsRadialPattern2, TRANSITION_4>
+
     >reactions;
 
   // STATE FUNCTIONS
@@ -57,7 +51,7 @@ struct StNavigateMineWaypointsX : smacc2::SmaccState<StNavigateMineWaypointsX, M
       // // configure_orthogonal<OrNavigation, CbPositionControlFreeSpace>();
       // configure_orthogonal<OrNavigation, CbNavigateNextWaypoint>();
 
-      configure_orthogonal<OrNavigation, CbLoadWaypointsFile>("waypoints_mine", "sm_dancebot_mine_ue");
+      // configure_orthogonal<OrNavigation, CbLoadWaypointsFile>("waypoints_mine", "sm_dancebot_mine_ue");
       configure_orthogonal<OrNavigation, CbNavigateNextWaypointFree>();
   }
 

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 <smacc2/smacc.hpp>
+
+namespace sm_dancebot_mine_ue
+{
+
+// STATE DECLARATION
+struct StNavigateReverseMineWaypointsX : smacc2::SmaccState<StNavigateReverseMineWaypointsX, MsDanceBotRunMode>
+{
+  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<cl_nav2z::EvWaypointFinal, StFinalState, SUCCESS>,
+    Transition<EvCbSuccess<CbNavigateNextWaypointFree, OrNavigation>, StNavigateReverseMineWaypointsX, TRANSITION_1>
+    >reactions;
+
+  // STATE FUNCTIONS
+  static void staticConfigure()
+  {
+      configure_orthogonal<OrNavigation, CbNavigateNextWaypointFree>();
+  }
+
+  void onEntry()
+  {
+  }
+
+  void runtimeConfigure() {}
+
+  void onExit(ABORT)
+  {
+  }
+};
+}  // namespace sm_dancebot_mine_ue

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 <smacc2/smacc.hpp>
+
+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<StTurnAround, MsDanceBotRunMode>
+{
+  using SmaccState::SmaccState;
+
+  // TRANSITION TABLE
+  typedef mpl::list<
+    // Transition<EvCbSuccess<CbPureSpinning, OrNavigation>, StNavigateReverseMineWaypointsX, SUCCESS>
+    Transition<EvCbSuccess<CbSleepFor, OrNavigation>, StNavigateReverseMineWaypointsX, SUCCESS>
+
+    >reactions;
+
+  // STATE FUNCTIONS
+  static void staticConfigure()
+  {
+    // configure_orthogonal<OrNavigation, CbPureSpinning>(M_PI, 0.8);
+    configure_orthogonal<OrNavigation,CbSleepFor>(2s);
+    configure_orthogonal<OrNavigation, sm_dancebot_mine_ue::CbLoadWaypointsFile>("waypoints_mine_reverse", "sm_dancebot_mine_ue");
+  }
+
+  void runtimeConfigure()
+  {
+    // auto spinningBehavior = this->getOrthogonal<OrNavigation>()->getClientBehavior<CbPureSpinning>();
+    // spinningBehavior->yaw_goal_tolerance_rads_ = 0.2;
+
+  }
+};
+}  // namespace sm_dancebot_ue

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