merging fix

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

smacc2_client_library/nav2z_client/nav2z_client/package.xml

@@ -19,7 +19,10 @@
   <depend>yaml_cpp_vendor</depend>
   <depend>nav2_msgs</depend>
   <depend>pluginlib</depend>
+  <depend>rclcpp_action</depend>
 
+  <depend>nav2_util</depend>
+
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
   <depend>tf2_ros</depend>

smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_active_stop.cpp

@@ -0,0 +1,53 @@
+// 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
+ *
+ ******************************************************************************************************************/
+
+#include <angles/angles.h>
+#include <geometry_msgs/msg/twist.hpp>
+#include <smacc2/smacc_asynchronous_client_behavior.hpp>
+
+#include <nav2z_client/client_behaviors/cb_active_stop.hpp>
+
+namespace cl_nav2z
+{
+CbActiveStop::CbActiveStop() {}
+
+void CbActiveStop::onEntry()
+{
+  auto nh = this->getNode();
+  cmd_vel_pub_ = nh->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", rclcpp::QoS(1));
+
+  rclcpp::Rate loop_rate(5);
+  geometry_msgs::msg::Twist cmd_vel_msg;
+  while (!this->isShutdownRequested())
+  {
+    cmd_vel_msg.linear.x = 0;
+    cmd_vel_msg.angular.z = 0;
+
+    cmd_vel_pub_->publish(cmd_vel_msg);
+    loop_rate.sleep();
+  }
+  RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Finished behavior execution");
+
+  this->postSuccessEvent();
+}
+
+void CbActiveStop::onExit() {}
+
+}  // namespace cl_nav2z