diff --git a/.clang-format b/.clang-format old mode 100644 new mode 100755 diff --git a/smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt b/smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt index 172c36af0..c39928bff 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..76395de1e --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_active_stop.hpp @@ -0,0 +1,40 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +#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..4f85bc0f4 --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_load_waypoints_file.hpp @@ -0,0 +1,46 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +#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..bcf5e392c --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_navigate_next_waypoint_free.hpp @@ -0,0 +1,44 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once +#include +#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..a76b814bf --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_position_control_free_space.hpp @@ -0,0 +1,64 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +#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..ffbc312b5 --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_pure_spinning.hpp @@ -0,0 +1,48 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +#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..0ef5ae9b3 --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_save_slam_map.hpp @@ -0,0 +1,46 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +#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..ea177ba37 --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_active_stop.cpp @@ -0,0 +1,53 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include +#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..13de11be8 --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_load_waypoints_file.cpp @@ -0,0 +1,57 @@ +// 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..02b7b8375 --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_next_waypoint_free.cpp @@ -0,0 +1,54 @@ +// 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..c0671a863 --- /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..19de603b1 --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_pure_spinning.cpp @@ -0,0 +1,102 @@ +// 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..6500bc8f3 --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_save_slam_map.cpp @@ -0,0 +1,85 @@ +// 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 + +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