From 4705f810edc84624c2de4b49bf978d02040cc947 Mon Sep 17 00:00:00 2001 From: bozhi Date: Fri, 24 Apr 2020 18:31:45 +0800 Subject: [PATCH] Add getRobotVelocity function and mbf_msgs/ExePathFeedback/actual_vel --- .../abstract_controller_execution.h | 9 +++++++++ .../abstract_navigation_server.h | 3 +++ .../include/mbf_abstract_nav/controller_action.h | 1 + .../include/mbf_abstract_nav/robot_information.h | 2 +- .../src/abstract_controller_execution.cpp | 13 ++++++++++--- .../src/abstract_navigation_server.cpp | 3 +++ mbf_abstract_nav/src/controller_action.cpp | 5 +++++ mbf_abstract_nav/src/robot_information.cpp | 7 ++++--- mbf_msgs/action/ExePath.action | 1 + .../include/mbf_utility/navigation_utility.h | 16 ++++++++++++++++ mbf_utility/src/navigation_utility.cpp | 15 +++++++++++++++ 11 files changed, 68 insertions(+), 7 deletions(-) diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h index 747fb6b2..41783a0f 100644 --- a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h +++ b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h @@ -49,6 +49,7 @@ #include #include #include +#include #include #include @@ -274,6 +275,11 @@ namespace mbf_abstract_nav */ bool computeRobotPose(); + /** + * @brief Update the robot actual velocity; + */ + inline void updateActualRobotVelocity(); + /** * @brief Sets the controller state. This method makes the communication of the state thread safe. * @param state The current controller state. @@ -358,6 +364,9 @@ namespace mbf_abstract_nav //! current robot pose; geometry_msgs::PoseStamped robot_pose_; + //! current robot actual velocity; + geometry_msgs::TwistStamped robot_velocity_; + //! whether check for action specific tolerance bool tolerance_from_action_; diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_navigation_server.h b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_navigation_server.h index 7bcd15a0..73f8aff2 100644 --- a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_navigation_server.h +++ b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_navigation_server.h @@ -347,6 +347,9 @@ typedef boost::shared_ptrcomputeVelocityCommands(robot_pose, robot_velocity, vel_cmd, message); + return controller_->computeVelocityCommands(robot_pose, robot_velocity_, vel_cmd, message); } @@ -343,6 +348,9 @@ void AbstractControllerExecution::run() // compute robot pose and store it in robot_pose_ computeRobotPose(); + // update robot actual velocity and store it in robot_velocity + updateActualRobotVelocity(); + // ask planner if the goal is reached if (reachedGoalCheck()) { @@ -367,8 +375,7 @@ void AbstractControllerExecution::run() // call plugin to compute the next velocity command geometry_msgs::TwistStamped cmd_vel_stamped; - geometry_msgs::TwistStamped robot_velocity; // TODO pass current velocity to the plugin! - outcome_ = computeVelocityCmd(robot_pose_, robot_velocity, cmd_vel_stamped, message_ = ""); + outcome_ = computeVelocityCmd(robot_pose_, robot_velocity_, cmd_vel_stamped, message_ = ""); if (outcome_ < 10) { diff --git a/mbf_abstract_nav/src/abstract_navigation_server.cpp b/mbf_abstract_nav/src/abstract_navigation_server.cpp index ba3558fb..19e33467 100644 --- a/mbf_abstract_nav/src/abstract_navigation_server.cpp +++ b/mbf_abstract_nav/src/abstract_navigation_server.cpp @@ -72,6 +72,9 @@ AbstractNavigationServer::AbstractNavigationServer(const TFPtr &tf_listener_ptr) // init cmd_vel publisher for the robot velocity vel_pub_ = nh.advertise("cmd_vel", 1); + // init odom_sub_ subscribe for the actual robot velocity + odom_sub_ = nh.subscribe("odom", 1, &mbf_utility::actualVelCallback); + action_server_get_path_ptr_ = ActionServerGetPathPtr( new ActionServerGetPath( private_nh_, diff --git a/mbf_abstract_nav/src/controller_action.cpp b/mbf_abstract_nav/src/controller_action.cpp index ed81d428..68b789e3 100644 --- a/mbf_abstract_nav/src/controller_action.cpp +++ b/mbf_abstract_nav/src/controller_action.cpp @@ -335,6 +335,7 @@ void ControllerAction::publishExePathFeedback( const geometry_msgs::TwistStamped& current_twist) { mbf_msgs::ExePathFeedback feedback; + feedback.outcome = outcome; feedback.message = message; @@ -342,6 +343,10 @@ void ControllerAction::publishExePathFeedback( if (feedback.last_cmd_vel.header.stamp.isZero()) feedback.last_cmd_vel.header.stamp = ros::Time::now(); + //TODO: maybe can consider the feedback error + robot_info_.getRobotVelocity(robot_actual_vel_); + feedback.actual_vel = robot_actual_vel_; + feedback.current_pose = robot_pose_; feedback.dist_to_goal = static_cast(mbf_utility::distance(robot_pose_, goal_pose_)); feedback.angle_to_goal = static_cast(mbf_utility::angle(robot_pose_, goal_pose_)); diff --git a/mbf_abstract_nav/src/robot_information.cpp b/mbf_abstract_nav/src/robot_information.cpp index b35195df..73454f52 100644 --- a/mbf_abstract_nav/src/robot_information.cpp +++ b/mbf_abstract_nav/src/robot_information.cpp @@ -66,10 +66,11 @@ bool RobotInformation::getRobotPose(geometry_msgs::PoseStamped &robot_pose) cons return true; } -bool RobotInformation::getRobotVelocity(geometry_msgs::TwistStamped &robot_velocity, ros::Duration look_back_duration) const +bool RobotInformation::getRobotVelocity(geometry_msgs::TwistStamped &robot_velocity) const { - // TODO implement and filter tf data to compute velocity. - return false; + //TODO: maybe can consider the feedback error + mbf_utility::getRobotActualVelocity(robot_velocity); + return true; } const std::string& RobotInformation::getGlobalFrame() const {return global_frame_;}; diff --git a/mbf_msgs/action/ExePath.action b/mbf_msgs/action/ExePath.action index 189ed8e0..6a63ea15 100644 --- a/mbf_msgs/action/ExePath.action +++ b/mbf_msgs/action/ExePath.action @@ -57,3 +57,4 @@ float32 dist_to_goal float32 angle_to_goal geometry_msgs/PoseStamped current_pose geometry_msgs/TwistStamped last_cmd_vel # last command calculated by the controller +geometry_msgs/TwistStamped actual_vel # robot actual velocity feedback from wheel encoder diff --git a/mbf_utility/include/mbf_utility/navigation_utility.h b/mbf_utility/include/mbf_utility/navigation_utility.h index 7f23f97e..0bf4b3f1 100644 --- a/mbf_utility/include/mbf_utility/navigation_utility.h +++ b/mbf_utility/include/mbf_utility/navigation_utility.h @@ -42,6 +42,8 @@ #define MBF_UTILITY__NAVIGATION_UTILITY_H_ #include +#include +#include #include #include #include @@ -107,6 +109,20 @@ bool getRobotPose(const TF &tf, const std::string &global_frame, const ros::Duration &timeout, geometry_msgs::PoseStamped &robot_pose); + +/** + * @brief subscribe odom_topic callback + * @param sub_odometry subscribe data from wheel odometry + */ +void actualVelCallback(const nav_msgs::Odometry::ConstPtr& sub_odometry); + +/** + * @brief update the robot actual velocity. + * @param tf_listener TransformListener. + * @return true, if succeeded, false otherwise. + */ +void getRobotActualVelocity(geometry_msgs::TwistStamped &robot_velocity); + /** * @brief Computes the Euclidean-distance between two poses. * @param pose1 pose 1 diff --git a/mbf_utility/src/navigation_utility.cpp b/mbf_utility/src/navigation_utility.cpp index abb0fabc..f38766a9 100644 --- a/mbf_utility/src/navigation_utility.cpp +++ b/mbf_utility/src/navigation_utility.cpp @@ -42,6 +42,8 @@ namespace mbf_utility { + geometry_msgs::TwistStamped actual_vel; + boost::mutex vel_mutex; bool getRobotPose(const TF &tf, const std::string &robot_frame, @@ -169,6 +171,19 @@ bool transformPoint(const TF &tf, return true; } +void actualVelCallback(const nav_msgs::Odometry::ConstPtr& sub_odometry) +{ + boost::lock_guard lock(vel_mutex); + actual_vel.header = sub_odometry->header; + actual_vel.twist = sub_odometry->twist.twist; +} + +void getRobotActualVelocity(geometry_msgs::TwistStamped &robot_velocity) +{ + boost::lock_guard lock(vel_mutex); + robot_velocity = actual_vel; +} + double distance(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2) { const geometry_msgs::Point &p1 = pose1.pose.position;