Brettpac branch

.github/workflows/ci-ros-lint.yaml

@@ -43,66 +43,7 @@ jobs:
           sm_atomic
           sm_atomic_performance_test
           sm_atomic_subscribers_performance_test
-          sm_dance_bot
-          sm_dance_bot_strikes_back
-          sm_ferrari
-          sm_three_some
 
           sr_all_events_go
           sr_conditional
           sr_event_countdown
-
-  #ament_lint_cpplint:
-    #name: ament_${{ matrix.linter }}
-    #runs-on: ubuntu-20.04
-    #strategy:
-      #fail-fast: false
-      #matrix:
-          #linter: [cpplint]
-    #steps:
-    #- uses: actions/checkout@v1
-    #- uses: ros-tooling/[email protected]
-    #- uses: ros-tooling/[email protected]
-      #with:
-        #distribution: galactic
-        #linter: cpplint
-        #arguments: "--filter=-whitespace/newline"
-        #package-name:
-          #smacc2
-
-          #keyboard_client
-
-          #backward_global_planner
-          #backward_local_planner
-          #forward_global_planner
-          #forward_local_planner
-          #nav2z_planners_common
-          #pure_spinning_local_planner
-          #undo_path_global_planner
-          #nav2z_client
-
-          #multirole_sensor_client
-          #ros_publisher_client
-          #ros_timer_client
-
-          #eg_conditional_generator
-          #eg_random_generator
-
-          #smacc2_msgs
-
-          #sm_atomic
-          #sm_atomic_performance_test
-          #sm_atomic_subscribers_performance_test
-          #sm_dance_bot
-          #sm_dance_bot_strikes_back
-          #sm_three_some
-
-          #sr_all_events_go
-          #sr_conditional
-          #sr_event_countdown
-
-          #smacc_client_library
-          #smacc_event_generators
-          #smacc_msgs
-          #smacc_sm_reference_library
-          #smacc_state_reactor_library

smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt

@@ -25,6 +25,9 @@ find_package(pluginlib REQUIRED)
 find_package(bond REQUIRED)
 find_package(slam_toolbox REQUIRED)
 find_package(ament_index_cpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(nav2_util)
+
 
 set(dependencies
   smacc2
@@ -39,6 +42,8 @@ set(dependencies
   bond
   slam_toolbox
   ament_index_cpp
+  rclcpp_action
+  nav2_util
 )
 
 include_directories(include)
@@ -79,6 +84,13 @@ add_library(${PROJECT_NAME} SHARED
   src/${PROJECT_NAME}/components/waypoints_navigator/cp_waypoints_visualizer.cpp
   src/${PROJECT_NAME}/components/amcl/cp_amcl.cpp
   src/${PROJECT_NAME}/components/slam_toolbox/cp_slam_toolbox.cpp
+
+  src/${PROJECT_NAME}/client_behaviors/cb_active_stop.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_load_waypoints_file.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_navigate_next_waypoint_free.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_position_control_free_space.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_pure_spinning.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_save_slam_map.cpp
 )
 
 ament_target_dependencies(${PROJECT_NAME}

smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_active_stop.hpp

@@ -0,0 +1,40 @@
+// 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 <geometry_msgs/msg/twist.hpp>
+#include <smacc2/smacc_asynchronous_client_behavior.hpp>
+
+namespace cl_nav2z
+{
+struct CbActiveStop : public smacc2::SmaccAsyncClientBehavior
+{
+private:
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
+
+public:
+  CbActiveStop();
+
+  void onEntry() override;
+
+  void onExit() override;
+};
+}  // namespace cl_nav2z

smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_load_waypoints_file.hpp

@@ -0,0 +1,46 @@
+// 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 <nav2z_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp>
+#include <smacc2/smacc_client_behavior.hpp>
+
+namespace cl_nav2z
+{
+struct CbLoadWaypointsFile : public smacc2::SmaccAsyncClientBehavior
+{
+public:
+  CbLoadWaypointsFile(std::string filepath);
+
+  CbLoadWaypointsFile(std::string parameter_name, std::string packagenamesapce);
+
+  void onEntry() override;
+
+  void onExit() override;
+
+  std::optional<std::string> filepath_;
+
+  std::optional<std::string> parameterName_;
+  std::optional<std::string> packageNamespace_;
+
+  cl_nav2z::CpWaypointNavigatorBase * waypointsNavigator_;
+};
+}  // namespace cl_nav2z

smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_navigate_next_waypoint_free.hpp

@@ -0,0 +1,44 @@
+// 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 <nav2z_client/client_behaviors/cb_position_control_free_space.hpp>
+#include <nav2z_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp>
+
+namespace cl_nav2z
+{
+
+class CbNavigateNextWaypointFree : public cl_nav2z::CbPositionControlFreeSpace
+{
+public:
+  CbNavigateNextWaypointFree();
+
+  virtual ~CbNavigateNextWaypointFree();
+
+  void onEntry() override;
+
+  void onSucessCallback();
+
+  void onExit() override;
+
+protected:
+  cl_nav2z::CpWaypointNavigatorBase * waypointsNavigator_;
+};
+
+}  // namespace cl_nav2z

smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_position_control_free_space.hpp

@@ -0,0 +1,64 @@
+// 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 <geometry_msgs/msg/twist.hpp>
+#include <nav2z_client/components/pose/cp_pose.hpp>
+#include <smacc2/smacc_asynchronous_client_behavior.hpp>
+
+namespace cl_nav2z
+{
+struct CbPositionControlFreeSpace : public smacc2::SmaccAsyncClientBehavior
+{
+private:
+  double targetYaw_;
+  bool goalReached_;
+  double k_betta_;
+  double max_angular_yaw_speed_;
+
+  double prev_error_linear_ = 0.0;
+  double prev_error_angular_ = 0.0;
+  double integral_linear_ = 0.0;
+  double integral_angular_ = 0.0;
+
+  // Limit the maximum linear velocity and angular velocity to avoid sudden
+  // movements
+  double max_linear_velocity = 1.0;   // Adjust this value according to your needs
+  double max_angular_velocity = 1.0;  // Adjust this value according to your needs
+
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
+
+public:
+  double yaw_goal_tolerance_rads_ = 0.1;
+
+  double threshold_distance_ = 3.0;
+
+  geometry_msgs::msg::Pose target_pose_;
+
+  CbPositionControlFreeSpace();
+
+  void updateParameters();
+
+  void onEntry() override;
+
+  void onExit() override;
+};
+}  // namespace cl_nav2z

smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_pure_spinning.hpp

@@ -0,0 +1,48 @@
+// 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 <geometry_msgs/msg/twist.hpp>
+#include <smacc2/smacc_asynchronous_client_behavior.hpp>
+
+namespace cl_nav2z
+{
+struct CbPureSpinning : public smacc2::SmaccAsyncClientBehavior
+{
+private:
+  double targetYaw__rads;
+  bool goalReached_;
+  double k_betta_;
+  double max_angular_yaw_speed_;
+
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
+
+public:
+  double yaw_goal_tolerance_rads_;
+
+  CbPureSpinning(double targetYaw_rads, double max_angular_yaw_speed = 0.5);
+  void updateParameters();
+
+  void onEntry() override;
+
+  void onExit() override;
+};
+}  // namespace cl_nav2z

smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_save_slam_map.hpp

@@ -0,0 +1,46 @@
+// 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 <memory>
+#include <nav2_msgs/srv/save_map.hpp>
+#include <slam_toolbox/srv/save_map.hpp>
+#include <smacc2/client_behaviors/cb_call_service.hpp>
+#include <smacc2/smacc_asynchronous_client_behavior.hpp>
+
+namespace cl_nav2z
+{
+using namespace std::chrono_literals;
+template <typename TService>
+using CbServiceCall = smacc2::client_behaviors::CbServiceCall<TService>;
+
+struct CbSaveSlamMap : public CbServiceCall<nav2_msgs::srv::SaveMap>
+{
+public:
+  CbSaveSlamMap();
+  // void onEntry() override {}
+
+  void onExit() override;
+
+  std::shared_ptr<nav2_msgs::srv::SaveMap::Request> getRequest(
+    /*slam_toolbox::srv::SaveMap_Request_<std::allocator<void> >::_name_type saved_map_name*/);
+};
+}  // namespace cl_nav2z