From 6f481a096e5c8c9ed68643fd6a274c3e0b501df1 Mon Sep 17 00:00:00 2001 From: brettpac Date: Tue, 23 Jul 2024 11:11:08 -0700 Subject: [PATCH] merging fix --- .../nav2z_client/nav2z_client/CMakeLists.txt | 12 ++ .../client_behaviors/cb_active_stop.hpp | 38 ++++ .../cb_load_waypoints_file.hpp | 44 ++++ .../cb_navigate_next_waypoint_free.hpp | 43 ++++ .../cb_position_control_free_space.hpp | 63 ++++++ .../client_behaviors/cb_pure_spinning.hpp | 46 ++++ .../client_behaviors/cb_save_slam_map.hpp | 47 +++++ .../nav2z_client/nav2z_client/package.xml | 3 + .../client_behaviors/cb_active_stop.cpp | 52 +++++ .../cb_load_waypoints_file.cpp | 51 +++++ .../cb_navigate_next_waypoint_free.cpp | 58 +++++ .../cb_position_control_free_space.cpp | 199 ++++++++++++++++++ .../client_behaviors/cb_pure_spinning.cpp | 104 +++++++++ .../client_behaviors/cb_save_slam_map.cpp | 87 ++++++++ .../odom_tracker/cp_odom_tracker_node.cpp | 2 +- 15 files changed, 848 insertions(+), 1 deletion(-) create mode 100644 smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_active_stop.hpp create mode 100644 smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_load_waypoints_file.hpp create mode 100644 smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_navigate_next_waypoint_free.hpp create mode 100644 smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_position_control_free_space.hpp create mode 100644 smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_pure_spinning.hpp create mode 100644 smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_save_slam_map.hpp create mode 100644 smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_active_stop.cpp create mode 100644 smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_load_waypoints_file.cpp create mode 100644 smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_next_waypoint_free.cpp create mode 100644 smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_position_control_free_space.cpp create mode 100644 smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_pure_spinning.cpp create mode 100644 smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_save_slam_map.cpp diff --git a/smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt b/smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt index 172c36af0..bcbd5d62f 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt +++ b/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} diff --git a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_active_stop.hpp b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_active_stop.hpp new file mode 100644 index 000000000..286662157 --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_active_stop.hpp @@ -0,0 +1,38 @@ +// 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 +#include + +namespace cl_nav2z { +struct CbActiveStop : public smacc2::SmaccAsyncClientBehavior { +private: + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + +public: + CbActiveStop(); + + void onEntry() override; + + void onExit() override; +}; +} // namespace cl_nav2z diff --git a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_load_waypoints_file.hpp b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_load_waypoints_file.hpp new file mode 100644 index 000000000..4844ffd59 --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_load_waypoints_file.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 +#include + +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 filepath_; + + std::optional parameterName_; + std::optional packageNamespace_; + + cl_nav2z::CpWaypointNavigatorBase *waypointsNavigator_; +}; +} // namespace cl_nav2z diff --git a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_navigate_next_waypoint_free.hpp b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_navigate_next_waypoint_free.hpp new file mode 100644 index 000000000..26c90d76f --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_navigate_next_waypoint_free.hpp @@ -0,0 +1,43 @@ +// 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 +#include + +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 diff --git a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_position_control_free_space.hpp b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_position_control_free_space.hpp new file mode 100644 index 000000000..e71a260d4 --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_position_control_free_space.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 +#include +#include + +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::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 diff --git a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_pure_spinning.hpp b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_pure_spinning.hpp new file mode 100644 index 000000000..cc830711f --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_pure_spinning.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 +#include + +namespace cl_nav2z { +struct CbPureSpinning : public smacc2::SmaccAsyncClientBehavior { +private: + double targetYaw__rads; + bool goalReached_; + double k_betta_; + double max_angular_yaw_speed_; + + rclcpp::Publisher::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 diff --git a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_save_slam_map.hpp b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_save_slam_map.hpp new file mode 100644 index 000000000..2f0356cf9 --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_save_slam_map.hpp @@ -0,0 +1,47 @@ +// 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 +#include +#include +#include +#include +#include + + +namespace cl_nav2z { +using namespace std::chrono_literals; +template +using CbServiceCall = smacc2::client_behaviors::CbServiceCall; + + +struct CbSaveSlamMap : public CbServiceCall { + +public: + CbSaveSlamMap(); + // void onEntry() override {} + + void onExit() override; + + std::shared_ptr getRequest(/*slam_toolbox::srv::SaveMap_Request_ >::_name_type saved_map_name*/) ; +}; +} // namespace cl_nav2z diff --git a/smacc2_client_library/nav2z_client/nav2z_client/package.xml b/smacc2_client_library/nav2z_client/nav2z_client/package.xml index e67f880a3..595a0a8fb 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/package.xml +++ b/smacc2_client_library/nav2z_client/nav2z_client/package.xml @@ -19,7 +19,10 @@ yaml_cpp_vendor nav2_msgs pluginlib + rclcpp_action + nav2_util + std_msgs std_srvs tf2_ros diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_active_stop.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_active_stop.cpp new file mode 100644 index 000000000..50bb1fd41 --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_active_stop.cpp @@ -0,0 +1,52 @@ +// 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 +#include +#include + +#include + +namespace cl_nav2z { +CbActiveStop::CbActiveStop() {} + +void CbActiveStop::onEntry() { + auto nh = this->getNode(); + cmd_vel_pub_ = nh->create_publisher( + "/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 diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_load_waypoints_file.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_load_waypoints_file.cpp new file mode 100644 index 000000000..4bb3e7dd2 --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_load_waypoints_file.cpp @@ -0,0 +1,51 @@ +// 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 +#include +#include + +namespace cl_nav2z { + + CbLoadWaypointsFile::CbLoadWaypointsFile(std::string filepath) : filepath_(filepath) {} + + CbLoadWaypointsFile::CbLoadWaypointsFile(std::string parameter_name, std::string packagenamesapce) + : parameterName_(parameter_name), packageNamespace_(packagenamesapce) {} + + void CbLoadWaypointsFile::onEntry() { + requiresComponent(waypointsNavigator_); // this is a component from the + // nav2z_client library + + if (filepath_) { + this->waypointsNavigator_->loadWayPointsFromFile(filepath_.value()); + } else { + RCLCPP_INFO(getLogger(), "Loading waypoints from parameter %s", + parameterName_.value().c_str()); + this->waypointsNavigator_->loadWaypointsFromYamlParameter( + parameterName_.value(), packageNamespace_.value()); + } + + // change this to skip some points of the yaml file, default = 0 + waypointsNavigator_->currentWaypoint_ = 0; + this->postSuccessEvent(); + } + + void CbLoadWaypointsFile::onExit() {} +} // namespace cl_nav2z diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_next_waypoint_free.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_next_waypoint_free.cpp new file mode 100644 index 000000000..f5637980a --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_next_waypoint_free.cpp @@ -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 + * + ******************************************************************************************************************/ +#include + + + namespace cl_nav2z { + + CbNavigateNextWaypointFree::CbNavigateNextWaypointFree() {} + + CbNavigateNextWaypointFree::~CbNavigateNextWaypointFree() {} + + void CbNavigateNextWaypointFree::onEntry() { + requiresComponent(this->waypointsNavigator_); + this->target_pose_ = this->waypointsNavigator_->getCurrentPose(); + + this->onSuccess(&CbNavigateNextWaypointFree::CbNavigateNextWaypointFree::onSucessCallback, this); + RCLCPP_INFO_STREAM( + getLogger(), + "[CbNavigateNextWaypoint] initial load file target pose: x: " + << this->target_pose_.position.x + << ", y: " << this->target_pose_.position.y); + CbPositionControlFreeSpace::onEntry(); + } + + void CbNavigateNextWaypointFree::onSucessCallback() { + RCLCPP_INFO_STREAM( + getLogger(), + "[CbNavigateNextWaypoint] Success on planning to next waypoint"); + this->waypointsNavigator_->notifyGoalReached(); + this->waypointsNavigator_->forward(1); + RCLCPP_INFO_STREAM( + getLogger(), "[CbNavigateNextWaypoint] next position index: " + << this->waypointsNavigator_->getCurrentWaypointIndex() + << "/" + << this->waypointsNavigator_->getWaypoints().size()); + } + + void CbNavigateNextWaypointFree::onExit() {} + + +} // namespace cl_nav2z diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_position_control_free_space.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_position_control_free_space.cpp new file mode 100644 index 000000000..7f789aba7 --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_position_control_free_space.cpp @@ -0,0 +1,199 @@ +// 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 +#include +#include + +#include +#include + +namespace cl_nav2z { +CbPositionControlFreeSpace::CbPositionControlFreeSpace() + : targetYaw_(0), k_betta_(1.0), max_angular_yaw_speed_(1.0) {} + +void CbPositionControlFreeSpace::updateParameters() {} + +void CbPositionControlFreeSpace::onEntry() { + auto nh = this->getNode(); + cmd_vel_pub_ = nh->create_publisher( + "/cmd_vel", rclcpp::QoS(1)); + + cl_nav2z::Pose *pose; + + this->requiresComponent(pose); + + geometry_msgs::msg::Twist cmd_vel; + goalReached_ = false; + + geometry_msgs::msg::Pose currentPose = pose->toPoseMsg(); + + rclcpp::Rate loop_rate(10); + double countAngle = 0; + auto prevyaw = tf2::getYaw(currentPose.orientation); + + // PID controller gains (proportional, integral, and derivative) + double kp_linear = 0.5; + double ki_linear = 0.0; + double kd_linear = 0.1; + + double kp_angular = 0.5; + double ki_angular = 0.0; + double kd_angular = 0.1; + + while (rclcpp::ok() && !goalReached_) { + RCLCPP_INFO_STREAM_THROTTLE(getLogger(), *getNode()->get_clock(), 200, + "CbPositionControlFreeSpace, current pose: " + << currentPose.position.x << ", " + << currentPose.position.y << ", " + << tf2::getYaw(currentPose.orientation)); + + RCLCPP_INFO_STREAM_THROTTLE(getLogger(), *getNode()->get_clock(), 200, + "CbPositionControlFreeSpace, target pose: " + << target_pose_.position.x << ", " + << target_pose_.position.y << ", " + << tf2::getYaw(target_pose_.orientation)); + + tf2::Quaternion q; + currentPose = pose->toPoseMsg(); + + // Here we implement the control logic to calculate the velocity command + // based on the received position of the vehicle and the target pose. + + // Calculate the errors in x and y + double error_x = target_pose_.position.x - currentPose.position.x; + double error_y = target_pose_.position.y - currentPose.position.y; + + // Calculate the distance to the target pose + double distance_to_target = + std::sqrt(error_x * error_x + error_y * error_y); + + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] distance to target: " + << distance_to_target << " ( th: " + << threshold_distance_ << ")"); + + // Check if the robot has reached the target pose + if (distance_to_target < threshold_distance_) { + RCLCPP_INFO(getLogger(), "Goal reached!"); + // Stop the robot by setting the velocity commands to zero + geometry_msgs::msg::Twist cmd_vel_msg; + cmd_vel_msg.linear.x = 0.0; + cmd_vel_msg.angular.z = 0.0; + cmd_vel_pub_->publish(cmd_vel_msg); + break; + } else { + // Calculate the desired orientation angle + double desired_yaw = std::atan2(error_y, error_x); + + // Calculate the difference between the desired orientation and the + // current orientation + double yaw_error = + desired_yaw - (tf2::getYaw(currentPose.orientation) + M_PI); + + // Ensure the yaw error is within the range [-pi, pi] + while (yaw_error > M_PI) + yaw_error -= 2 * M_PI; + while (yaw_error < -M_PI) + yaw_error += 2 * M_PI; + + // Calculate the control signals (velocity commands) using PID controllers + double cmd_linear_x = + kp_linear * distance_to_target + ki_linear * integral_linear_ + + kd_linear * (distance_to_target - prev_error_linear_); + double cmd_angular_z = kp_angular * yaw_error + + ki_angular * integral_angular_ + + kd_angular * (yaw_error - prev_error_angular_); + + if (cmd_linear_x > max_linear_velocity) + cmd_linear_x = max_linear_velocity; + else if (cmd_linear_x < -max_linear_velocity) + cmd_linear_x = -max_linear_velocity; + + if (cmd_angular_z > max_angular_velocity) + cmd_angular_z = max_angular_velocity; + else if (cmd_angular_z < -max_angular_velocity) + cmd_angular_z = -max_angular_velocity; + + // Construct and publish the velocity command message + geometry_msgs::msg::Twist cmd_vel_msg; + + cmd_vel_msg.linear.x = cmd_linear_x; + cmd_vel_msg.angular.z = cmd_angular_z; + + cmd_vel_pub_->publish(cmd_vel_msg); + + // Update errors and integrals for the next control cycle + prev_error_linear_ = distance_to_target; + prev_error_angular_ = yaw_error; + integral_linear_ += distance_to_target; + integral_angular_ += yaw_error; + + // tf2::fromMsg(currentPose.orientation, q); + // auto currentYaw = tf2::getYaw(currentPose.orientation); + // auto deltaAngle = angles::shortest_angular_distance(prevyaw, + // currentYaw); countAngle += deltaAngle; + + // prevyaw = currentYaw; + // double angular_error = targetYaw_ - countAngle; + + // auto omega = angular_error * k_betta_; + // cmd_vel.linear.x = 0; + // cmd_vel.linear.y = 0; + // cmd_vel.linear.z = 0; + // cmd_vel.angular.z = + // std::min(std::max(omega, -fabs(max_angular_yaw_speed_)), + // fabs(max_angular_yaw_speed_)); + + // RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] delta angle: " + // << deltaAngle); RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] + // cummulated angle: " << countAngle); RCLCPP_INFO_STREAM(getLogger(), "[" + // << getName() << "] k_betta_: " << k_betta_); + + // RCLCPP_INFO_STREAM( + // getLogger(), "[" << getName() << "] angular error: " << angular_error + // << "(" + // << yaw_goal_tolerance_rads_ << ")"); + // RCLCPP_INFO_STREAM( + // getLogger(), "[" << getName() << "] command angular speed: " << + // cmd_vel.angular.z); + + // if (fabs(angular_error) < yaw_goal_tolerance_rads_) + // { + // RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] GOAL REACHED. + // Sending stop command."); goalReached_ = true; cmd_vel.linear.x = 0; + // cmd_vel.angular.z = 0; + // break; + // } + + // this->cmd_vel_pub_->publish(cmd_vel); + + loop_rate.sleep(); + } + } + + RCLCPP_INFO_STREAM(getLogger(), + "[" << getName() << "] Finished behavior execution"); + + this->postSuccessEvent(); +} + +void CbPositionControlFreeSpace::onExit() {} + +} // namespace cl_nav2z diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_pure_spinning.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_pure_spinning.cpp new file mode 100644 index 000000000..5c264deaf --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_pure_spinning.cpp @@ -0,0 +1,104 @@ +// 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 +#include +#include +#include +#include +namespace cl_nav2z { + + + CbPureSpinning::CbPureSpinning(double targetYaw_rads, double max_angular_yaw_speed) + : targetYaw__rads(targetYaw_rads), k_betta_(1.0), + max_angular_yaw_speed_(max_angular_yaw_speed), + yaw_goal_tolerance_rads_(0.1) {} + + void CbPureSpinning::updateParameters() {} + + void CbPureSpinning::onEntry() { + auto nh = this->getNode(); + cmd_vel_pub_ = nh->create_publisher( + "/cmd_vel", rclcpp::QoS(1)); + + cl_nav2z::Pose *pose; + this->requiresComponent(pose); + + geometry_msgs::msg::Twist cmd_vel; + goalReached_ = false; + + geometry_msgs::msg::Pose currentPose = pose->toPoseMsg(); + + rclcpp::Rate loop_rate(10); + double countAngle = 0; + auto prevyaw = tf2::getYaw(currentPose.orientation); + while (rclcpp::ok() && !goalReached_) { + tf2::Quaternion q; + currentPose = pose->toPoseMsg(); + tf2::fromMsg(currentPose.orientation, q); + auto currentYaw = tf2::getYaw(currentPose.orientation); + auto deltaAngle = angles::shortest_angular_distance(prevyaw, currentYaw); + countAngle += deltaAngle; + + prevyaw = currentYaw; + double angular_error = targetYaw__rads - countAngle; + + auto omega = angular_error * k_betta_; + cmd_vel.linear.x = 0; + cmd_vel.linear.y = 0; + cmd_vel.linear.z = 0; + cmd_vel.angular.z = + std::min(std::max(omega, -fabs(max_angular_yaw_speed_)), + fabs(max_angular_yaw_speed_)); + + RCLCPP_INFO_STREAM(getLogger(), + "[" << getName() << "] delta angle: " << deltaAngle); + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] cummulated angle: " + << countAngle); + RCLCPP_INFO_STREAM(getLogger(), + "[" << getName() << "] k_betta_: " << k_betta_); + + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] angular error: " + << angular_error << "(" + << yaw_goal_tolerance_rads_ << ")"); + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() + << "] command angular speed: " + << cmd_vel.angular.z); + + if (fabs(angular_error) < yaw_goal_tolerance_rads_) { + RCLCPP_INFO_STREAM(getLogger(), + "[" << getName() + << "] GOAL REACHED. Sending stop command."); + goalReached_ = true; + cmd_vel.linear.x = 0; + cmd_vel.angular.z = 0; + break; + } + + this->cmd_vel_pub_->publish(cmd_vel); + + loop_rate.sleep(); + } + + this->postSuccessEvent(); + } + + void CbPureSpinning::onExit() {} +} // namespace cl_nav2z diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_save_slam_map.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_save_slam_map.cpp new file mode 100644 index 000000000..5b5e9d473 --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_save_slam_map.cpp @@ -0,0 +1,87 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace cl_nav2z { +using namespace std::chrono_literals; + + + CbSaveSlamMap::CbSaveSlamMap(): CbServiceCall("/map_saver/save_map", + getRequest()) { + // : CbServiceCall("/slam_toolbox/save_map", + // getRequest()) { + + // map_name.data = "saved_map"; + // auto request = getRequest(map_name); + // RCLCPP_INFO_STREAM(getLogger(), "Save Slam Map builded"); + } + + // void onEntry() override {} + + void CbSaveSlamMap::onExit() {} + + std::shared_ptr CbSaveSlamMap::getRequest(/*slam_toolbox::srv::SaveMap_Request_ >::_name_type saved_map_name*/) { + nav2_msgs::srv::SaveMap_Request map_save; + std_msgs::msg::String map_name; + + // // map_name.data = "saved_map"; + // map_save.map_topic = "map"; + // map_save.map_url = "${workspacesFolder}/maps/saved_map"; + // map_save.image_format = "png"; + // map_save.occupied_thresh = 0.65; + // map_save.free_thresh = 0.25; + // map_save.map_mode = "trinary"; + + // // auto request = std::make_shared(); + // // // request->name = saved_map_name; + // // request->name = map_name; + // // return request; + // auto request = std::make_shared(map_save); + + auto request = std::make_shared(); + request->map_topic = "map"; + request->map_url = "/tmp/saved_map"; + request->image_format = "png"; + request->occupied_thresh = 0.65; + request->free_thresh = 0.25; + request->map_mode = "trinary"; + + + return request; +} +} // namespace cl_nav2z + + +// slam_toolbox::srv::SaveMap_Request_ >::_name_type + +// std_msgs::msg::String_ > diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/odom_tracker/cp_odom_tracker_node.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/odom_tracker/cp_odom_tracker_node.cpp index 8b55efe95..0e64c1c22 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/odom_tracker/cp_odom_tracker_node.cpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/odom_tracker/cp_odom_tracker_node.cpp @@ -17,7 +17,7 @@ * Authors: Pablo Inigo Blasco, Brett Aldrich * ******************************************************************************************************************/ -#include +//#include #include #include #include