Amcl auto localization #504

mhpanah · 2018-12-19T00:39:06Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#503
Primary OS tested on	Ubuntu 18.04
Robotic platform tested on	gazebo simulation with turtlebot

Description of contribution in a few bullet points

Note: I will fix the CI failure before merge.

Implemented Simple AutoLocalization with BT for differential type robots.
Figure below shows the AutoLocalization BT branch.

Future work that may be required in bullet points

Expand it to other type of robots
Currently the spin and back_up motion primitives are time based. Will need to modify the tree once the motion primitives support distance and angle.

ghost · 2018-12-19T17:03:40Z

Can you add images of the BTs? You should be able to create the image in Groot or your tool of choice.

You could either check in the image where the xml file is, or just add it to the PR description

SteveMacenski · 2018-12-19T19:39:16Z

nav2_tasks/include/nav2_tasks/is_localized_condition.hpp

+
+    // Naive way to check if the robot has been localized
+    // TODO(mhpanah): come up with a method to properly check particles convergence
+    if (current_pose->pose.covariance[0] < 0.25 &&


We shouldnt be hard coding magic numbers, can we parameterize this? This covariance value as localized is highly tied to the amcl or particle filter configuration

Agreed. However, currently I haven't decide how to pass these parameters yet (i.e. from xml file or parameter server). This will be addressed in a different PR. Also, this BT branch is designed to work with AMCL and differential type robots and I have a TODO to make this more generic. Basically, for now this is an additional feature for AMCL.

You can make it a parameter on the node to start with. That way all you have to do is a simple get_parameter_or during initialization to get the configurable parameters. The non-configurable parameters, like the covariance array indexes perhaps, can just be constants you define - maybe in the class definition in this case.

bpwilcox

Looks good to me, I agree about parameterizing the covariance values in future PR. Looks like the build isn't passing yet though.

orduno · 2018-12-20T00:57:16Z

nav2_tasks/include/nav2_tasks/global_localization_service_client.hpp

+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_TASKS__GLOBAL_LOCLIZATION_SERVICE_CLIENT_HPP_


Typo in this line

orduno · 2018-12-20T01:04:12Z

nav2_bt_navigator/src/bt_navigator.cpp

@@ -16,6 +16,7 @@
 #include <memory>
 #include <fstream>
 #include <streambuf>
+#include <set>


Looks like you're not using set but including it to remove the linter error. Use `\ NOLINT' instead.

orduno · 2018-12-20T01:04:42Z

nav2_bt_navigator/src/bt_navigator.cpp

@@ -52,6 +53,7 @@ BtNavigator::navigateToPose(const nav2_tasks::NavigateToPoseCommand::SharedPtr c
  // Set the shared data (commands/results)
  blackboard->set<nav2_tasks::ComputePathToPoseCommand::SharedPtr>("goal", command);
  blackboard->set<nav2_tasks::ComputePathToPoseResult::SharedPtr>("path", path);  // NOLINT
+  blackboard->set<bool>("initial_pose", task_server_->hasInitialPoseReceived());


Add \\ NOLINT at the end of the line, see line 55.

orduno · 2018-12-20T01:08:44Z

nav2_tasks/include/nav2_tasks/is_localized_condition.hpp

+
+      robot_ = std::make_unique<nav2_robot::Robot>(node_);
+
+      onInit();


I don't think this is needed. Unless you're planning on having other classes derived from IsLocalizedCondition.

orduno · 2018-12-20T01:12:28Z

nav2_tasks/include/nav2_tasks/is_localized_condition.hpp

+      // Get the required items from the blackboard
+      node_ = blackboard()->template get<rclcpp::Node::SharedPtr>("node");
+
+      node_loop_timeout_ =


Do you need this variable?

orduno · 2018-12-20T01:14:33Z

nav2_tasks/include/nav2_tasks/global_localization_service_client.hpp

+
+}  // namespace nav2_tasks
+
+#endif  // NAV2_TASKS__GLOBAL_LOCLIZATION_SERVICE_CLIENT_HPP_


Typo in this line.

orduno · 2018-12-20T01:21:25Z

nav2_tasks/include/nav2_tasks/global_localization_service_client.hpp

+namespace nav2_tasks
+{
+
+class globalLocalizationServiceClient : public ServiceClient<std_srvs::srv::Empty>


Can we clarify a bit more what this service is doing? I see we are sending and receiving empty messages.

This sends an empty msg request to invoke amcl's global localization service call.

orduno · 2018-12-20T01:27:22Z

nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp

+  }
+}
+
+BT::NodeStatus NavigateToPoseBehaviorTree::initialPoseReceived(BT::TreeNode & tree_node)


This is the first node ticked on the BT, it seems that with this logic it will always returns FAILURE. Are we checking somewhere else if the user has set the initial position elsewhere? for example in RVIZ.

It will return Success, when there is an initial pose. This method will be updated once we enable reading initial pose from parameter server.

orduno · 2018-12-20T01:28:51Z

nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp

+
+BT::NodeStatus NavigateToPoseBehaviorTree::initialPoseReceived(BT::TreeNode & tree_node)
+{
+  auto initPose = tree_node.blackboard()->template get<bool>("initial_pose");


Eventually I think we should check if the initial pose was received through the robot class.

I'm halting adding any new code in robot class until team decides what to do with robot class.

orduno · 2018-12-20T01:37:25Z

nav2_tasks/include/nav2_tasks/navigate_to_pose_task.hpp

@@ -39,22 +40,46 @@ class NavigateToPoseTaskServer : public TaskServer<NavigateToPoseCommand, Naviga
    goal_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>("move_base_simple/goal",
        std::bind(&NavigateToPoseTaskServer::onGoalPoseReceived, this, std::placeholders::_1));

+    initial_pose_received_ = false;
+    initial_pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(


Maybe this could be done through the robot class.

orduno · 2018-12-20T01:38:19Z

nav2_tasks/include/nav2_tasks/navigate_to_pose_task.hpp

  void onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose)
  {
    self_client_->sendCommand(pose);
  }
+  void onInitialPoseReceived(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr /*msg*/)
+  {


Why not store the message with the initial pose we've received?

The initial pose msg is not needed here.

orduno · 2018-12-20T01:41:01Z

nav2_tasks/include/nav2_tasks/navigate_to_pose_task.hpp

+    return false;
+  }
+
+  void setInitialPose(bool initial_pose)


if initial_pose_ is false, we don't need to pass an argument to make it true.

In this case we do. When AutoLocalization is done, this flags needs to be changed so that on next cycle we know that the robot has already been localized.

orduno

I've made some minor recommendations.

ghost · 2018-12-20T17:09:37Z

nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp

+
+BT::NodeStatus NavigateToPoseBehaviorTree::initialPoseReceived(BT::TreeNode & tree_node)
+{
+  auto initPose = tree_node.blackboard()->template get<bool>("initial_pose");


These variables - initPose and "initial_pose" sound like they contain a pose, not a boolean. Can we change them to something like initial_pose_received" or something?

ghost · 2018-12-20T17:17:24Z

nav2_tasks/include/nav2_tasks/navigate_to_pose_task.hpp

    // A client that we'll use to send a command message to our own task server
    self_client_ = std::make_unique<nav2_tasks::NavigateToPoseTaskClient>(node_);
  }

  NavigateToPoseTaskServer() = delete;

+  bool hasInitialPoseReceived()


I'd go for isInitialPoseReceieved rather than has. I think that is more standard for a function that returns a boolean.

ghost · 2018-12-20T17:26:09Z

nav2_tasks/include/nav2_tasks/is_localized_condition.hpp

+
+    // Naive way to check if the robot has been localized
+    // TODO(mhpanah): come up with a method to properly check particles convergence
+    if (current_pose->pose.covariance[0] < 0.25 &&


You can make it a parameter on the node to start with. That way all you have to do is a simple get_parameter_or during initialization to get the configurable parameters. The non-configurable parameters, like the covariance array indexes perhaps, can just be constants you define - maybe in the class definition in this case.

ghost · 2018-12-20T17:31:09Z

@mhpanah How will this fit into the overall btNavigator? How does this interact with manually setting the initial pose in rviz? How does this interact with loading the initial pose from a parameter server?

mhpanah · 2018-12-20T17:43:48Z

@mhpanah How will this fit into the overall btNavigator? How does this interact with manually setting the initial pose in rviz? How does this interact with loading the initial pose from a parameter server?

I'm creating a README to explain how it fits into the overall btNavigator
This subscribes to initial_pose topic which is the topic that rviz is publishing on.
Currently we are getting the initial pose from rviz and we don't have a way to get it from the parameter server yet. Once, we enabled getting the pose from parameter server we can update the check initial pose to see if the initial pose is on the parameter server similar to how it's done in amcl.

* update docs Signed-off-by: gg <[email protected]> * updates Signed-off-by: gg <[email protected]> --------- Signed-off-by: gg <[email protected]>

mhpanah added 7 commits December 18, 2018 11:13

added globalLocalization Service Client

8a8bbbd

Added check to see if the initial pose has been received.

96eefb9

adding is_localized_condition to BT for AutoLocalization

e95b56b

added auto_loclization tree

2d84a97

fixed linters and changed current_pose ROS_WARN to ROS_DEBUG

3152318

Added AutoLocalization with Parallel and Recovery BT

2ddc5da

setting the initial pose flag after autolocalization

4ccdad2

mhpanah added the 1 - High High Priority label Dec 19, 2018

mhpanah added this to the D Turtle Release milestone Dec 19, 2018

mhpanah self-assigned this Dec 19, 2018

mhpanah requested review from bpwilcox, a user and orduno December 19, 2018 00:39

SteveMacenski requested changes Dec 19, 2018

View reviewed changes

SteveMacenski approved these changes Dec 19, 2018

View reviewed changes

bpwilcox approved these changes Dec 20, 2018

View reviewed changes

orduno reviewed Dec 20, 2018

View reviewed changes

orduno suggested changes Dec 20, 2018

View reviewed changes

ghost reviewed Dec 20, 2018

View reviewed changes

Addressing reviewers comments

e5134e4

mhpanah force-pushed the amcl-autoLocalization branch from 59468e6 to e5134e4 Compare December 20, 2018 22:38

mhpanah merged commit 0124bc0 into ros-navigation:master Dec 20, 2018

mhpanah mentioned this pull request Dec 20, 2018

Auto initial pose localization for differential drive robots #503

Closed

mhpanah mentioned this pull request Jan 3, 2019

[WIP] Add amcl's service request to robot class #427

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Amcl auto localization #504

Amcl auto localization #504

mhpanah commented Dec 19, 2018 •

edited

Loading

ghost commented Dec 19, 2018

SteveMacenski Dec 19, 2018

mhpanah Dec 19, 2018 •

edited

Loading

ghost Dec 20, 2018

bpwilcox left a comment •

edited

Loading

orduno Dec 20, 2018

orduno Dec 20, 2018

orduno Dec 20, 2018

orduno Dec 20, 2018 •

edited

Loading

orduno Dec 20, 2018

orduno Dec 20, 2018

orduno Dec 20, 2018

mhpanah Dec 20, 2018

orduno Dec 20, 2018

mhpanah Dec 20, 2018

orduno Dec 20, 2018

mhpanah Dec 20, 2018

orduno Dec 20, 2018

orduno Dec 20, 2018

mhpanah Dec 20, 2018

orduno Dec 20, 2018

mhpanah Dec 20, 2018

orduno left a comment

ghost Dec 20, 2018

ghost Dec 20, 2018

ghost Dec 20, 2018

ghost commented Dec 20, 2018

mhpanah commented Dec 20, 2018


		robot_ = std::make_unique<nav2_robot::Robot>(node_);

		onInit();


		} // namespace nav2_tasks

		#endif // NAV2_TASKS__GLOBAL_LOCLIZATION_SERVICE_CLIENT_HPP_

Amcl auto localization #504

Amcl auto localization #504

Conversation

mhpanah commented Dec 19, 2018 • edited Loading

Basic Info

Description of contribution in a few bullet points

Future work that may be required in bullet points

ghost commented Dec 19, 2018

Choose a reason for hiding this comment

mhpanah Dec 19, 2018 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

bpwilcox left a comment • 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

orduno Dec 20, 2018 • 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

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

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

orduno 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

ghost commented Dec 20, 2018

mhpanah commented Dec 20, 2018

mhpanah commented Dec 19, 2018 •

edited

Loading

mhpanah Dec 19, 2018 •

edited

Loading

bpwilcox left a comment •

edited

Loading

orduno Dec 20, 2018 •

edited

Loading