From 16acb5ef433f63c17bd599f192da0317eaa4c575 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Tue, 1 Mar 2022 09:33:16 +0000 Subject: [PATCH 1/2] Refactored output parameter. --- .../path_tracking_pid_local_planner.hpp | 5 ++--- src/path_tracking_pid_local_planner.cpp | 17 +++++++++-------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index 627b2143..fbc0a63e 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -90,10 +90,9 @@ class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController, private: /** * @brief Calculates the velocity command based on the current robot pose given by pose. - * @param cmd_vel Output the velocity command - * @return true if succeded. + * @return Velocity command on success, empty optional otherwise. */ - bool computeVelocityCommands(geometry_msgs::Twist & cmd_vel); + std::optional computeVelocityCommands(); /** * @brief Returns true, if the goal is reached. diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 84a28ec4..d79b1c04 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -209,7 +209,7 @@ bool TrackingPidLocalPlanner::setPlan(const std::vector TrackingPidLocalPlanner::computeVelocityCommands() { ros::Time now = ros::Time::now(); if (prev_time_.isZero()) { @@ -219,14 +219,14 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist & cmd if (dt.isZero()) { ROS_ERROR_THROTTLE( 5, "dt=0 detected, skipping loop(s). Possible overloaded cpu or simulating too fast"); - cmd_vel = geometry_msgs::Twist(); + auto cmd_vel = geometry_msgs::Twist(); cmd_vel.linear.x = pid_controller_.getControllerState().current_x_vel; cmd_vel.angular.z = pid_controller_.getControllerState().current_yaw_vel; - return true; // False is no use: https://github.com/magazino/move_base_flex/issues/195 + return cmd_vel; } if (dt < ros::Duration(0) || dt > ros::Duration(DT_MAX)) { ROS_ERROR("Invalid time increment: %f. Aborting", dt.toSec()); - return false; + return std::nullopt; } try { ROS_DEBUG("map_frame: %s, base_link_frame: %s", map_frame_.c_str(), base_link_frame_.c_str()); @@ -234,7 +234,7 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist & cmd } catch (const tf2::TransformException & ex) { ROS_ERROR("Received an exception trying to transform: %s", ex.what()); active_goal_ = false; - return false; + return std::nullopt; } // Handle obstacles @@ -258,7 +258,6 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist & cmd const auto update_result = pid_controller_.update_with_limits( tf2_convert(tfCurPoseStamped_.transform), latest_odom_.twist.twist, dt); - cmd_vel = update_result.velocity_command; path_tracking_pid::PidFeedback feedback_msg; feedback_msg.eda = ros::Duration(update_result.eda); @@ -296,7 +295,7 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist & cmd prev_time_ = now; prev_dt_ = dt; // Store last known valid dt for next cycles (https://github.com/magazino/move_base_flex/issues/195) - return true; + return update_result.velocity_command; } uint8_t TrackingPidLocalPlanner::projectedCollisionCost() @@ -441,10 +440,12 @@ uint32_t TrackingPidLocalPlanner::computeVelocityCommands( return mbf_msgs::ExePathResult::NOT_INITIALIZED; } // TODO(Cesar): Use provided pose and odom - if (!computeVelocityCommands(cmd_vel.twist)) { + const auto opt_cmd_vel = computeVelocityCommands(); + if (!opt_cmd_vel) { active_goal_ = false; return mbf_msgs::ExePathResult::FAILURE; } + cmd_vel.twist = *opt_cmd_vel; cmd_vel.header.stamp = ros::Time::now(); cmd_vel.header.frame_id = base_link_frame_; From c615c89eaf31f8c41618a0e18c25da006fdad9ad Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 3 Mar 2022 13:43:18 +0000 Subject: [PATCH 2/2] Reworked by adding comment. --- src/path_tracking_pid_local_planner.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index d79b1c04..556912ec 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -222,6 +222,9 @@ std::optional TrackingPidLocalPlanner::computeVelocityComm auto cmd_vel = geometry_msgs::Twist(); cmd_vel.linear.x = pid_controller_.getControllerState().current_x_vel; cmd_vel.angular.z = pid_controller_.getControllerState().current_yaw_vel; + // At the first call of computeVelocityCommands() we can't calculate a cmd_vel. We can't return + // false because of https://github.com/magazino/move_base_flex/issues/195 so the current + // velocity is send instead. return cmd_vel; } if (dt < ros::Duration(0) || dt > ros::Duration(DT_MAX)) {