From 6f481a096e5c8c9ed68643fd6a274c3e0b501df1 Mon Sep 17 00:00:00 2001 From: brettpac Date: Tue, 23 Jul 2024 11:11:08 -0700 Subject: [PATCH 1/3] 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 From b634d1f773bee675578766bf17296c61a72fed96 Mon Sep 17 00:00:00 2001 From: brettpac Date: Tue, 23 Jul 2024 13:59:50 -0700 Subject: [PATCH 2/3] fixing format error --- .clang-format | 0 .github/workflows/ci-ros-lint.yaml | 4 +- .../nav2z_client/nav2z_client/CMakeLists.txt | 8 +- .../client_behaviors/cb_active_stop.hpp | 8 +- .../cb_load_waypoints_file.hpp | 12 +- .../cb_navigate_next_waypoint_free.hpp | 13 +- .../cb_position_control_free_space.hpp | 13 +- .../client_behaviors/cb_pure_spinning.hpp | 8 +- .../client_behaviors/cb_save_slam_map.hpp | 17 +- .../client_behaviors/cb_active_stop.cpp | 17 +- .../cb_load_waypoints_file.cpp | 52 +++--- .../cb_navigate_next_waypoint_free.cpp | 70 ++++----- .../cb_position_control_free_space.cpp | 78 ++++----- .../client_behaviors/cb_pure_spinning.cpp | 148 +++++++++--------- .../client_behaviors/cb_save_slam_map.cpp | 90 ++++++----- 15 files changed, 272 insertions(+), 266 deletions(-) mode change 100644 => 100755 .clang-format diff --git a/.clang-format b/.clang-format old mode 100644 new mode 100755 diff --git a/.github/workflows/ci-ros-lint.yaml b/.github/workflows/ci-ros-lint.yaml index 6e1a06608..d3c094e93 100644 --- a/.github/workflows/ci-ros-lint.yaml +++ b/.github/workflows/ci-ros-lint.yaml @@ -5,7 +5,7 @@ on: jobs: ament_lint: name: ament_${{ matrix.linter }} - runs-on: ubuntu-20.04 + runs-on: ubuntu-latest strategy: fail-fast: false matrix: @@ -15,7 +15,7 @@ jobs: - uses: ros-tooling/setup-ros@v0.2 - uses: ros-tooling/action-ros-lint@v0.1 with: - distribution: galactic + distribution: humble linter: ${{ matrix.linter }} package-name: smacc2 diff --git a/smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt b/smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt index bcbd5d62f..c39928bff 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt +++ b/smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt @@ -85,11 +85,11 @@ add_library(${PROJECT_NAME} SHARED 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_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_pure_spinning.cpp src/${PROJECT_NAME}/client_behaviors/cb_save_slam_map.cpp ) 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 index 286662157..76395de1e 100644 --- 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 @@ -23,8 +23,10 @@ #include #include -namespace cl_nav2z { -struct CbActiveStop : public smacc2::SmaccAsyncClientBehavior { +namespace cl_nav2z +{ +struct CbActiveStop : public smacc2::SmaccAsyncClientBehavior +{ private: rclcpp::Publisher::SharedPtr cmd_vel_pub_; @@ -35,4 +37,4 @@ struct CbActiveStop : public smacc2::SmaccAsyncClientBehavior { void onExit() override; }; -} // namespace cl_nav2z +} // 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 index 4844ffd59..4f85bc0f4 100644 --- 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 @@ -23,10 +23,12 @@ #include #include -namespace cl_nav2z { -struct CbLoadWaypointsFile : public smacc2::SmaccAsyncClientBehavior { +namespace cl_nav2z +{ +struct CbLoadWaypointsFile : public smacc2::SmaccAsyncClientBehavior +{ public: - CbLoadWaypointsFile(std::string filepath) ; + CbLoadWaypointsFile(std::string filepath); CbLoadWaypointsFile(std::string parameter_name, std::string packagenamesapce); @@ -39,6 +41,6 @@ struct CbLoadWaypointsFile : public smacc2::SmaccAsyncClientBehavior { std::optional parameterName_; std::optional packageNamespace_; - cl_nav2z::CpWaypointNavigatorBase *waypointsNavigator_; + cl_nav2z::CpWaypointNavigatorBase * waypointsNavigator_; }; -} // namespace cl_nav2z +} // 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 index 26c90d76f..bcf5e392c 100644 --- 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 @@ -18,13 +18,14 @@ * ******************************************************************************************************************/ #pragma once -#include #include +#include -namespace cl_nav2z { +namespace cl_nav2z +{ -class CbNavigateNextWaypointFree - : public cl_nav2z::CbPositionControlFreeSpace { +class CbNavigateNextWaypointFree : public cl_nav2z::CbPositionControlFreeSpace +{ public: CbNavigateNextWaypointFree(); @@ -37,7 +38,7 @@ class CbNavigateNextWaypointFree void onExit() override; protected: - cl_nav2z::CpWaypointNavigatorBase *waypointsNavigator_; + cl_nav2z::CpWaypointNavigatorBase * waypointsNavigator_; }; -} // namespace cl_nav2z +} // 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 index e71a260d4..a76b814bf 100644 --- 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 @@ -24,8 +24,10 @@ #include #include -namespace cl_nav2z { -struct CbPositionControlFreeSpace : public smacc2::SmaccAsyncClientBehavior { +namespace cl_nav2z +{ +struct CbPositionControlFreeSpace : public smacc2::SmaccAsyncClientBehavior +{ private: double targetYaw_; bool goalReached_; @@ -39,9 +41,8 @@ struct CbPositionControlFreeSpace : public smacc2::SmaccAsyncClientBehavior { // 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 + 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_; @@ -60,4 +61,4 @@ struct CbPositionControlFreeSpace : public smacc2::SmaccAsyncClientBehavior { void onExit() override; }; -} // namespace cl_nav2z +} // 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 index cc830711f..ffbc312b5 100644 --- 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 @@ -23,8 +23,10 @@ #include #include -namespace cl_nav2z { -struct CbPureSpinning : public smacc2::SmaccAsyncClientBehavior { +namespace cl_nav2z +{ +struct CbPureSpinning : public smacc2::SmaccAsyncClientBehavior +{ private: double targetYaw__rads; bool goalReached_; @@ -43,4 +45,4 @@ struct CbPureSpinning : public smacc2::SmaccAsyncClientBehavior { void onExit() override; }; -} // namespace cl_nav2z +} // 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 index 2f0356cf9..0ef5ae9b3 100644 --- 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 @@ -23,25 +23,24 @@ #include #include #include -#include #include #include - -namespace cl_nav2z { +namespace cl_nav2z +{ using namespace std::chrono_literals; template using CbServiceCall = smacc2::client_behaviors::CbServiceCall; - -struct CbSaveSlamMap : public CbServiceCall { - -public: +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*/) ; + std::shared_ptr getRequest( + /*slam_toolbox::srv::SaveMap_Request_ >::_name_type saved_map_name*/); }; -} // namespace cl_nav2z +} // namespace cl_nav2z 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 index 50bb1fd41..ea177ba37 100644 --- 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 @@ -24,29 +24,30 @@ #include -namespace cl_nav2z { +namespace cl_nav2z +{ CbActiveStop::CbActiveStop() {} -void CbActiveStop::onEntry() { +void CbActiveStop::onEntry() +{ auto nh = this->getNode(); - cmd_vel_pub_ = nh->create_publisher( - "/cmd_vel", rclcpp::QoS(1)); + 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()) { + 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"); + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Finished behavior execution"); this->postSuccessEvent(); } void CbActiveStop::onExit() {} -} // namespace cl_nav2z +} // 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 index 4bb3e7dd2..13de11be8 100644 --- 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 @@ -18,34 +18,40 @@ * ******************************************************************************************************************/ -#include +#include #include -#include - -namespace cl_nav2z { +#include - CbLoadWaypointsFile::CbLoadWaypointsFile(std::string filepath) : filepath_(filepath) {} +namespace cl_nav2z +{ - CbLoadWaypointsFile::CbLoadWaypointsFile(std::string parameter_name, std::string packagenamesapce) - : parameterName_(parameter_name), packageNamespace_(packagenamesapce) {} +CbLoadWaypointsFile::CbLoadWaypointsFile(std::string filepath) : filepath_(filepath) {} - void CbLoadWaypointsFile::onEntry() { - requiresComponent(waypointsNavigator_); // this is a component from the - // nav2z_client library +CbLoadWaypointsFile::CbLoadWaypointsFile(std::string parameter_name, std::string packagenamesapce) +: parameterName_(parameter_name), packageNamespace_(packagenamesapce) +{ +} - 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()); - } +void CbLoadWaypointsFile::onEntry() +{ + requiresComponent(waypointsNavigator_); // this is a component from the + // nav2z_client library - // change this to skip some points of the yaml file, default = 0 - waypointsNavigator_->currentWaypoint_ = 0; - this->postSuccessEvent(); + 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 +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 index f5637980a..02b7b8375 100644 --- 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 @@ -19,40 +19,36 @@ ******************************************************************************************************************/ #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 +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 index 7f789aba7..c0671a863 100644 --- 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 @@ -22,21 +22,24 @@ #include #include -#include #include +#include -namespace cl_nav2z { +namespace cl_nav2z +{ CbPositionControlFreeSpace::CbPositionControlFreeSpace() - : targetYaw_(0), k_betta_(1.0), max_angular_yaw_speed_(1.0) {} +: targetYaw_(0), k_betta_(1.0), max_angular_yaw_speed_(1.0) +{ +} void CbPositionControlFreeSpace::updateParameters() {} -void CbPositionControlFreeSpace::onEntry() { +void CbPositionControlFreeSpace::onEntry() +{ auto nh = this->getNode(); - cmd_vel_pub_ = nh->create_publisher( - "/cmd_vel", rclcpp::QoS(1)); + cmd_vel_pub_ = nh->create_publisher("/cmd_vel", rclcpp::QoS(1)); - cl_nav2z::Pose *pose; + cl_nav2z::Pose * pose; this->requiresComponent(pose); @@ -58,18 +61,19 @@ void CbPositionControlFreeSpace::onEntry() { 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)); + 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)); + 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(); @@ -82,15 +86,15 @@ void CbPositionControlFreeSpace::onEntry() { 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); + 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_ << ")"); + 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_) { + 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; @@ -98,27 +102,24 @@ void CbPositionControlFreeSpace::onEntry() { cmd_vel_msg.angular.z = 0.0; cmd_vel_pub_->publish(cmd_vel_msg); break; - } else { + } + 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); + 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; + 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_ + + 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) @@ -188,12 +189,11 @@ void CbPositionControlFreeSpace::onEntry() { } } - RCLCPP_INFO_STREAM(getLogger(), - "[" << getName() << "] Finished behavior execution"); + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Finished behavior execution"); this->postSuccessEvent(); } void CbPositionControlFreeSpace::onExit() {} -} // namespace cl_nav2z +} // 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 index 5c264deaf..19de603b1 100644 --- 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 @@ -20,85 +20,83 @@ #include #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_; +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.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(); + cmd_vel.angular.z = 0; + break; } - this->postSuccessEvent(); + this->cmd_vel_pub_->publish(cmd_vel); + + loop_rate.sleep(); } - void CbPureSpinning::onExit() {} -} // namespace cl_nav2z + 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 index 5b5e9d473..6500bc8f3 100644 --- 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 @@ -18,69 +18,67 @@ * ******************************************************************************************************************/ -#include -#include -#include -#include -#include #include +#include #include +#include +#include +#include #include #include #include #include -#include - +#include -namespace cl_nav2z { +namespace cl_nav2z +{ using namespace std::chrono_literals; - - CbSaveSlamMap::CbSaveSlamMap(): CbServiceCall("/map_saver/save_map", - getRequest()) { - // : CbServiceCall("/slam_toolbox/save_map", - // getRequest()) { +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"); - } + // map_name.data = "saved_map"; + // auto request = getRequest(map_name); + // RCLCPP_INFO_STREAM(getLogger(), "Save Slam Map builded"); +} - // void onEntry() override {} +// void onEntry() override {} - void CbSaveSlamMap::onExit() {} +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"; +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; - // // auto request = std::make_shared(); - // // // request->name = saved_map_name; - // // request->name = map_name; - // // return request; - // auto request = std::make_shared(map_save); + // // 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->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"; - + // // auto request = std::make_shared(); + // // // request->name = saved_map_name; + // // request->name = map_name; + // // return request; + // auto request = std::make_shared(map_save); - return request; -} -} // namespace cl_nav2z + 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 From 32172a7ec9e0efbdde0f0322c9a5d0e8ffcbe562 Mon Sep 17 00:00:00 2001 From: brettpac Date: Tue, 23 Jul 2024 14:32:50 -0700 Subject: [PATCH 3/3] trying to fix linter --- .github/workflows/ci-ros-lint.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci-ros-lint.yaml b/.github/workflows/ci-ros-lint.yaml index d3c094e93..6e1a06608 100644 --- a/.github/workflows/ci-ros-lint.yaml +++ b/.github/workflows/ci-ros-lint.yaml @@ -5,7 +5,7 @@ on: jobs: ament_lint: name: ament_${{ matrix.linter }} - runs-on: ubuntu-latest + runs-on: ubuntu-20.04 strategy: fail-fast: false matrix: @@ -15,7 +15,7 @@ jobs: - uses: ros-tooling/setup-ros@v0.2 - uses: ros-tooling/action-ros-lint@v0.1 with: - distribution: humble + distribution: galactic linter: ${{ matrix.linter }} package-name: smacc2