Add getRobotVelocity function and mbf_msgs/ExePathFeedback/actual_vel #189

q576333 · 2020-04-24T10:54:56Z

I have Implemented getRobotVelocity function in the RobotInformation class and updateActualRobotVelocity function in AbstractControllerExecution class and tested. Let CostmapController plugin interface can get actual robot_velocity in computeVelocityCommands() function.
Also I add new ExePath action feedback variable "feedback_cmd_vel". I think maybe useful for some action client scenario. But I notice this feedback_cmd_vel have timing issue with the computeVelocityCommands() function's robot_velocity.
In addition, There is some warning message when I ctrl + c the move_base_flex terminal. I need everyone help me to check my PR is correct or not?
The warning message is "Warning: class_loader.ClassLoader: SEVERE WARNING!!! Attempting to unload library while objects created by this loader exist in the heap! You should delete your objects before attempting to unload the library or destroying the ClassLoader. The library will NOT be unloaded. at line 108 in /tmp/binarydeb/ros-kinetic-class-loader-0.3.9/src/class_loader.cpp"

mbf_abstract_nav/src/abstract_navigation_server.cpp

mbf_abstract_nav/include/mbf_abstract_nav/abstract_navigation_server.h

mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h

corot · 2020-04-28T02:36:52Z

mbf_abstract_nav/include/mbf_abstract_nav/controller_action.h

@@ -91,6 +91,7 @@ class ControllerAction :

  boost::mutex goal_mtx_; ///< lock goal handle for updating it while running
  geometry_msgs::PoseStamped robot_pose_; ///< Current robot pose
+  geometry_msgs::TwistStamped robot_feedback_vel_; //Actual robot velocity feedback


better robot_vel_; ///< Actual robot velocity feedback
(why feedback?; and use inline docstring)

In control field normally call sensor information be a feedback. I am worried that will not be able to clearly indicate the meaning of this robot_vel_ in the program (like cmd_vel or actual velocity). maybe follow the same naming rule call robot_actual_vel_?

I would use current_pose_ and current_vel_, as in the ExePath feedback msg, but put both attributes in the RobotInformation class (and move the odom topic there, too, as I explain in the review)

corot · 2020-04-28T02:38:18Z

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 feedback_cmd_vel  # robot actual velocity feedback from wheel encoder


I don't like feedback... what about current? or actual?

What about odom_vel_?

I prefer odom_vel_ too. but I think ExePath action's feedback is for user to monitor each mobile robot behavior, so maybe follow whole naming rule, call actual_vel_ ?

current_vel, and move above last_cmd_vel;
mostly to match current_pose; both are physical properties:

geometry_msgs/PoseStamped current_pose geometry_msgs/TwistStamped current_vel # robot actual velocity from the odometry geometry_msgs/TwistStamped last_cmd_vel # last command calculated by the controller

btw, given this interface change, and according to #191, this change won't be ever released 😞

Jap, this line should be removed here. So we can not change the messages anymore. For noetic I think this will be ok, if we hurry a bit, but not for melodic and before.

mbf_utility/include/mbf_utility/navigation_utility.h

mbf_utility/src/navigation_utility.cpp

corot · 2020-05-12T02:09:13Z

Hi @q576333, are you still working on this change? Please rebase master and address the review comments if so.

As for the Warning: class_loader.ClassLoader, don't worry, it always happen when using plugins in ROS, not only on MBF.

q576333 · 2020-05-22T07:54:19Z

Sorry, recently I working hard my master thesis and also this is my first pull requests, so let me to understand how to rebase master, sorry again for keeping you waiting so long.

corot · 2020-06-10T11:25:55Z

mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h

+    /**
+     * @brief Update the robot actual velocity;
+     */
+    inline void updateActualRobotVelocity();


why inline?

corot · 2020-06-10T11:27:27Z

mbf_utility/include/mbf_utility/navigation_utility.h

+ * @brief subscribe odom_topic callback
+ * @param sub_odometry subscribe data from wheel odometry
+ */           
+void actualVelCallback(const nav_msgs::Odometry::ConstPtr& sub_odometry);


The callback is for the odom topic, so... should be odometryCallback

corot · 2020-06-10T11:28:20Z

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<geometry_msgs::Twist>("cmd_vel", 1);

+  // init odom_sub_ subscribe for the actual robot velocity
+  odom_sub_ = nh.subscribe<nav_msgs::Odometry>("odom", 1, &mbf_utility::actualVelCallback);


Why not subscribing on mbf_utility/include/mbf_utility/navigation_utility.h, if the callback is there?

corot

Still need some work.
Moreover, seems clear to me that the odom subscription should be in RobotInformation class. And that it should be updated on demand, so the updateActualRobotVelocity new method is not necessary. But we can let this for a further refactoring.

FabianSchurig · 2020-06-11T20:01:33Z

mbf_utility/include/mbf_utility/navigation_utility.h

+/**
+ * @brief update the robot actual velocity.
+ * @param tf_listener TransformListener.
+ * @return true, if succeeded, false otherwise.


FabianSchurig · 2020-06-11T20:09:59Z

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


dont forget to rename this as well

Jap, this line should be removed here. So we can not change the messages anymore. For noetic I think this will be ok, if we hurry a bit, but not for melodic and before. See #191

spuetz

Please remove the change from the action message for now. I would also prefer to not use the Robot Information. Keep things simple here: Add the subscriber to the AbstractControllerExecution. This also has a hidden advantage of the namespace and using different odom topics.

corot · 2020-06-12T11:08:26Z

Please remove the change from the action message for now. I would also prefer to not use the Robot Information. Keep things simple here: Add the subscriber to the AbstractControllerExecution. This also has a hidden advantage of the namespace and using different odom topics.

Why not concentrating all robot's state-related code on RobotInformation (and keep the executions simple)? We already handled current pose there, so... handling velocity too sounds quite reasonable to me. But maybe we are making poor @q576333 dizzy... we can merge this with just the requested changes and refactor later with whatever we decide.

spuetz · 2020-06-12T16:44:23Z

But we are just receiving an odom message and hand it over to the plugins, I think it is a bit overkill and it looks hacky from my point of view. Furthermore, considering one mbf instance for multiple robots in the future a separate namespace is beneficial.

spuetz · 2020-06-12T16:47:04Z

I'll implement this in the next week and open a new PR, then we can decide which one is better.

spuetz · 2020-11-05T11:44:32Z

I'll address this in another PR.

corot requested changes Apr 28, 2020

View reviewed changes

spuetz self-assigned this Apr 30, 2020

spuetz added the enhancement label Apr 30, 2020

spuetz force-pushed the master branch from 8182898 to 2657775 Compare May 25, 2020 09:30

q576333 force-pushed the master branch from 837a698 to 93ffc36 Compare May 26, 2020 01:00

Add getRobotVelocity function and mbf_msgs/ExePathFeedback/actual_vel

4705f81

q576333 force-pushed the master branch from 93ffc36 to 4705f81 Compare May 26, 2020 01:02

q576333 changed the title ~~Add getRobotVelocity function and mbf_msgs/ExePathFeedback/feedback_cmd_vel~~ Add getRobotVelocity function and mbf_msgs/ExePathFeedback/actual_vel May 26, 2020

q576333 requested a review from corot June 10, 2020 08:24

corot reviewed Jun 10, 2020

View reviewed changes

corot requested changes Jun 10, 2020

View reviewed changes

FabianSchurig reviewed Jun 11, 2020

View reviewed changes

spuetz requested changes Jun 12, 2020

View reviewed changes

corot mentioned this pull request Jun 23, 2020

Compute current velocity for the controller. #34

Closed

This was referenced Jul 17, 2020

Check the absolute angle difference when checking angular tolerance #211

Closed

MBF must ensure that the robot is stopped when checking for goal reached #213

Open

spuetz closed this Nov 5, 2020

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add getRobotVelocity function and mbf_msgs/ExePathFeedback/actual_vel #189

Add getRobotVelocity function and mbf_msgs/ExePathFeedback/actual_vel #189

q576333 commented Apr 24, 2020

corot Apr 28, 2020

q576333 May 22, 2020 •

edited

Loading

corot Jun 10, 2020

corot Apr 28, 2020

spuetz Apr 28, 2020

q576333 May 22, 2020

corot Jun 10, 2020 •

edited

Loading

spuetz Jun 12, 2020

corot commented May 12, 2020 •

edited

Loading

q576333 commented May 22, 2020

corot Jun 10, 2020

corot Jun 10, 2020

corot Jun 10, 2020

corot left a comment

FabianSchurig Jun 11, 2020

FabianSchurig Jun 11, 2020

spuetz Jun 12, 2020

spuetz left a comment

corot commented Jun 12, 2020 •

edited

Loading

spuetz commented Jun 12, 2020

spuetz commented Jun 12, 2020 •

edited

Loading

spuetz commented Nov 5, 2020

Add getRobotVelocity function and mbf_msgs/ExePathFeedback/actual_vel #189

Add getRobotVelocity function and mbf_msgs/ExePathFeedback/actual_vel #189

Conversation

q576333 commented Apr 24, 2020

Choose a reason for hiding this comment

q576333 May 22, 2020 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

corot Jun 10, 2020 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

corot commented May 12, 2020 • edited Loading

q576333 commented May 22, 2020

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

corot left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

spuetz left a comment

Choose a reason for hiding this comment

corot commented Jun 12, 2020 • edited Loading

spuetz commented Jun 12, 2020

spuetz commented Jun 12, 2020 • edited Loading

spuetz commented Nov 5, 2020

q576333 May 22, 2020 •

edited

Loading

corot Jun 10, 2020 •

edited

Loading

corot commented May 12, 2020 •

edited

Loading

corot commented Jun 12, 2020 •

edited

Loading

spuetz commented Jun 12, 2020 •

edited

Loading