Merge development work to the master branch. #38

mjeronimo · 2018-08-30T20:48:16Z

This is early work to get a basic architectural shell in place, based on a task hierarchy concept.

* Add the requirements document * Update up-front material in requirements doc based on review feedback * Update Mission Planning and Execution sections * Begin integration of separate sections into a Planning section * Work on the Planning requirements section * More work on the Mapping section * Change Control -> Execution * Update the Planning and Execution sections * Address more review feedback * Address Matt's feedback * Address Matt's feedback * Check in an initial sketch of the mission execution code * Begin adding some sample state machine implementation code * Add the state machine transitions * Add some ROS messaging input to the MissionExecution * Begin sketching other levels of the stack * Remove include directory at the top level * Remove NavigationCommand in favor of NavigationTask * Add mission_planning directory and a few minor edits * Add a Doxyfile; add RosRobot class * Fix issues from lint and uncrustify * Add the architecture model to the repo * Add a cmake file for the robot directory * Make some changes to the Task hierarchy * Update some comments in Task about our approach

More work on basic command chain class hierarchy.

* Move some of the task base classes out to separate directories. * Implement start/stop for TaskClient and TaskServer

…sk hierarchy (#12) Implement the basic structure for the command chain modules.

* Cleaning up build exports and imports. * Adding auto uncrustify and cppcheck to the build. This will automatically run uncrustify, cppcheck, and cpplint whenever you run the test suite (eg. colcon test) Also, somewhat unrelated, I added whitespace to package.xml to make it a bit easier to read.

* Created the DijkstraPlanner based on the core algorithms from Navfn ROS1 nav package. * Updated the SimpleNavigator to use the new DijkstraPlanner * Created an initial WorldModel node and Costmap mockup to test the new planner. * Fixed bug with TaskClient. * Added a few new messages and services related to Costmap. Made a few updates to the Path messages.

#28) * Modify the directory structure to allow for multiple implementations of a given task type. Also create a shell BtNavigator class and introduce namespaces.

src/mission_execution/package.xml

mkhansenbot

For the empty files:
CHANGELOG.rst
README.md
.gitignore

Should we have these in every package directory, or just at the top level of the repo?

Doxyfile

mkhansenbot

I believe the file names should conform to the Google standard:
https://google.github.io/styleguide/cppguide.html#File_Names
The ROS and ROS2 Style guides seem to follow that. So MissionExecutor.hpp would become mission_executor.hpp

MJ: I've updated the filenames.

mkhansenbot · 2018-08-30T22:58:31Z

src/mission_execution/include/mission_execution/MissionExecution.hpp

+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright 2018 Intel Corporation. All Rights Reserved.
+
+#ifndef MISSION_EXECUTION__MISSIONEXECUTION_HPP_


Per Google's style guide:
https://google.github.io/styleguide/cppguide.html#The__define_Guard

Probably should rename the file to mission_execution.hpp and the define guard to MISSION_EXECUTION__MISSION_EXECUTION_HPP_

mkhansenbot · 2018-08-30T23:01:49Z

src/mission_execution/src/MissionExecution.cpp

+
+MissionExecution::~MissionExecution()
+{
+  RCLCPP_INFO(get_logger(), "MissionExecution::~MissionExecution");


Could we make this a little more user friendly log information like "Mission Execution Node is shutting down" or something like that?

mkhansenbot · 2018-08-30T23:03:12Z

src/mission_execution/src/MissionExecution.cpp

+void
+MissionExecution::executeMission(const MissionPlan * missionPlan)
+{
+  RCLCPP_INFO(get_logger(), "MissionExecution::executeMission");


Similarly can we make this "Executing Mission Plan"?

mkhansenbot · 2018-08-30T23:03:44Z

src/mission_execution/src/MissionExecution.cpp

+void
+MissionExecution::cancelMission()
+{
+  RCLCPP_INFO(get_logger(), "MissionExecution::cancelMission");


"Cancelling Mission"

mkhansenbot · 2018-08-30T23:33:06Z

src/mission_execution/src/MissionExecutor.cpp

+MissionExecutor::MissionExecutor(const std::string & name)
+: nav2_tasks::ExecuteMissionTaskServer(name)
+{
+  RCLCPP_INFO(get_logger(), "MissionExecutor::MissionExecutor");


Can we change these RCLCPP_INFO statements to something like "Starting Mission Executor", "Shutting down..." etc.

Sure. These were just to ensure everything was created and wired up correctly. We can go back and make them more meaningful as we implement the modules.

mkhansenbot · 2018-08-30T23:34:42Z

src/mission_execution/src/MissionExecutor.cpp

+        break;
+
+      default:
+        RCLCPP_INFO(get_logger(), "MissionExecutor::executeAsync: invalid status value");


Should this be RCLCPP_ERROR instead of INFO?

Yes, fixed.

mkhansenbot · 2018-08-30T23:41:05Z

src/nav2_tasks/include/nav2_tasks/TaskServer.hpp

+  {
+    std::cout << "onCommandReceived\n";
+    commandMsg_ = msg;
+    shouldExecute_ = true;


Design question - I'm not sure I understand why a thread is needed at all, couldn't the OnCommandReceived() method call the WorkerThread() method directly instead of setting a flag here?

I'm working under the assumption that we don't want to use one of the ROS threads for doing potentially long-running work. Instead, we want to do the minimum and return the thread to ROS for handling other input messages. I don't know if there's a convention or guidance in the ROS community for this.

I'll check what rclcpp::executor is doing. I was assuming it spawns a thread for every callback it makes, but I could be wrong.

mkhansenbot · 2018-08-30T23:50:17Z

I haven't finished reviewing, will do more tomorrow.

mkhansenbot · 2018-08-31T20:15:22Z

src/nav2_tasks/include/nav2_tasks/task_client.hpp

+  }
+
+  // The client can tell the TaskServer to execute its operation
+  void executeAsync(const typename CommandMsg::SharedPtr msg)


I think this would be better named "sendAsyncCommand", or as they do in rclcpp services "send_async_request". executeAsync is a bit confusing (at least to me) as the execution is actually on the server side. We're just requesting/commanding that execution to begin.

* Fix race conditions with the TaskClient and TaskServer templates * Code formatting per cpplint and uncrustify * Remove empty test directories for tasks * Address initial PR (#38) feedback. * Incorporate Carl's recommendations on TaskClient/Server

mkhansenbot · 2018-08-31T20:25:58Z

src/nav2_navigation/simple_navigator/src/simple_navigator.cpp

+    switch (status) {
+      case TaskStatus::SUCCEEDED:
+        RCLCPP_INFO(get_logger(), "SimpleNavigator::execute: planning task completed");
+        goto here;


Why is this a goto and not a break?

If it really needs to be a goto, can we at least rename the label from "here" to "task_succeeded" or something similar that is self documenting?

mkhansenbot · 2018-08-31T20:55:43Z

src/nav2_tasks/include/nav2_tasks/TaskClient.hpp

@@ -0,0 +1,173 @@
+// Copyright (c) 2018 Intel Corporation


I think the TaskClient.hpp file was accidentally added back in when the last commit was pushed. It was previously renamed to task_client.hpp, right? Now they're both in.

mkhansenbot · 2018-08-31T21:29:52Z

src/mission_execution/src/mission_executor.cpp

+: nav2_tasks::ExecuteMissionTaskServer(name)
+{
+  RCLCPP_INFO(get_logger(), "MissionExecutor::MissionExecutor");
+  navigationTask_ = std::make_unique<nav2_tasks::NavigateToPoseTaskClient>("SimpleNavigator", this);


Minor nit - calling this client "SimpleNavigator" is confusing since the SimpleNavigator class is a server. I guess this is the client side to that, but it could be the client side to any Navigation Server right? Or do I not understand correctly? If it is a client and the server can be provided by something other than the SimpleNavigator server, then can it be "NavigationClient"

src/control/nav2_dwa_controller/src/dwa_controller.cpp

mkhansenbot · 2018-08-31T21:51:47Z

src/mission_execution/nav2_mission_executor/src/mission_executor.cpp

+: nav2_tasks::ExecuteMissionTaskServer(name)
+{
+  RCLCPP_INFO(get_logger(), "MissionExecutor::MissionExecutor");
+  navigationTask_ = std::make_unique<nav2_tasks::NavigateToPoseTaskClient>("SimpleNavigator", this);


"SimpleNavigator" should be "NavigationClient" or something similar.

src/mission_execution/nav2_mission_executor/src/mission_executor.cpp

mkhansenbot · 2018-08-31T21:57:20Z

src/navigation/nav2_bt_navigator/src/bt_navigator.cpp

+: nav2_tasks::NavigateToPoseTaskServer(name)
+{
+  RCLCPP_INFO(get_logger(), "BtNavigator::BtNavigator");
+  planner_ = std::make_unique<nav2_tasks::ComputePathToPoseTaskClient>("AStarPlanner", this);


Should "AStarPlanner" be "PlannerClient" and similarly "DwaController" be "FollowPathClient"? Or am I missing something? I don't think the client is tied to a specific implementation of the server, is it?

Yes, you're right. These should be interface names rather than specific implementation names (as it is now). I'll change this.

… library

…el to costmap_world_model to be consistent with the directory/naming conventions

mkhansenbot

Lets file issues to track the few comments I made above, then we can merge this.

src/control/nav2_dwa_controller/package.xml

src/mission_execution/nav2_mission_executor/src/mission_executor.cpp

src/navigation/nav2_bt_navigator/src/bt_navigator.cpp

src/navigation/nav2_simple_navigator/src/simple_navigator.cpp

src/planning/nav2_dijkstra_planner/src/Logger.h

src/navigation/nav2_simple_navigator/src/simple_navigator.cpp

src/mission_execution/nav2_mission_executor/src/mission_executor.cpp

src/libs/nav2_tasks/include/nav2_tasks/task_client.hpp

crdelsey · 2018-09-05T22:21:42Z

src/libs/nav2_tasks/include/nav2_tasks/task_client.hpp

+    auto startTime = std::chrono::high_resolution_clock::now();
+
+    while (node_->count_subscribers(taskName) < 1) {
+      rclcpp::spin_some(node_->get_node_base_interface());


Can't you just pass in the node here? That's how the spin* functions normally work.

src/libs/nav2_tasks/include/nav2_tasks/task_client.hpp

crdelsey · 2018-09-05T22:32:14Z

src/libs/nav2_tasks/include/nav2_tasks/task_client.hpp

        std::bind(&TaskClient::onStatusReceived, this, std::placeholders::_1));
  }

+  TaskClient() = delete;


Is this the style we want to use? The default constructor is already gone because you specified an explicit constructor. This line is unnecessary, but maybe desirable for documentation purposes. My preference is to leave it out.

crdelsey · 2018-09-05T22:36:41Z

src/control/nav2_dwa_controller/src/dwa_controller.cpp

@@ -28,6 +28,12 @@ DwaController::DwaController(const std::string & name)
  RCLCPP_INFO(get_logger(), "DwaController::DwaController");
 }

+DwaController::DwaController()


Why not just make the name a default parameter to the first constructor?

crdelsey · 2018-09-05T22:37:20Z

src/control/nav2_dwa_controller/include/nav2_dwa_controller/dwa_controller.hpp

@@ -25,7 +25,7 @@ class DwaController : public nav2_tasks::FollowPathTaskServer
 {
 public:
  explicit DwaController(const std::string & name);
-  DwaController() = delete;
+  DwaController();


Why is the constructor with a name explicit, but not the constructor without a name?

crdelsey · 2018-09-05T22:43:48Z

src/navigation/nav2_simple_navigator/src/simple_navigator.cpp

@@ -79,7 +79,7 @@ SimpleNavigator::execute(const nav2_tasks::NavigateToPoseCommand::SharedPtr /*co
    switch (status) {
      case TaskStatus::SUCCEEDED:
        RCLCPP_INFO(get_logger(), "SimpleNavigator::execute: planning task completed");
-        goto here;
+        goto planning_succeeded;


I think this function should be restructured to eliminate the goto.

src/control/nav2_dwa_controller/package.xml

mkhansenbot

Latest changes look good to me. I'm good with merging this as long as you've filed issues for any open items.

* obstacle avoidance fine tune * revert bin number * add log that tells which motion primitives are used * change dx, dy * typo * finetune

Michael Jeronimo and others added 10 commits July 24, 2018 09:16

Ros2 devel (#8)

69a647c

More work on basic command chain class hierarchy.

Move some of the task base classes out to separate directories. (#9)

b7c30bb

Start implementation of TaskClient and TaskServer (#10)

435db94

* Move some of the task base classes out to separate directories. * Implement start/stop for TaskClient and TaskServer

Continue implementation of the architectural shell for the command ta…

f05decc

…sk hierarchy (#12) Implement the basic structure for the command chain modules.

Modify the directory structure to allow for multiple implementations … (

67c1e66

#28) * Modify the directory structure to allow for multiple implementations of a given task type. Also create a shell BtNavigator class and introduce namespaces.

A couple fixes missed in the previous integration check-in

958be39

Remove empty test directories for tasks

abb74db

mjeronimo requested a review from mkhansenbot August 30, 2018 20:48

crdelsey reviewed Aug 30, 2018

View reviewed changes

src/mission_execution/package.xml Outdated Show resolved Hide resolved

mkhansenbot reviewed Aug 30, 2018

View reviewed changes

Doxyfile Show resolved Hide resolved

mkhansenbot reviewed Aug 30, 2018

View reviewed changes

Address initial PR (#38) feedback.

07d7827

mkhansenbot reviewed Aug 31, 2018

View reviewed changes

Reorganize the directory structure

f2f5520

mkhansenbot reviewed Aug 31, 2018

View reviewed changes

src/control/nav2_dwa_controller/src/dwa_controller.cpp Show resolved Hide resolved

mkhansenbot reviewed Aug 31, 2018

View reviewed changes

src/mission_execution/nav2_mission_executor/src/mission_executor.cpp Show resolved Hide resolved

mkhansenbot reviewed Aug 31, 2018

View reviewed changes

Michael Jeronimo added 11 commits August 31, 2018 15:41

Move the costmap messages to be with the costmap class in the utility…

6cc167d

… library

Move messages out from nav2_tasks to their associated msg projects

eff830a

Consolidate and change the naming convention for the task headers

f1758ac

Remove some extraneous includes

ef6d854

Rename TaskClient's executeAsync method to sendCommand

29eec5b

Implement default names for tasks. Also, change the name of world_mod…

41c5af2

…el to costmap_world_model to be consistent with the directory/naming conventions

Fix uncrustify issues

3bc32a9

Introduce default names for tasks

1676622

Use consistent naming for task client member variables

74b6f76

Add a waitForServer function to the TaskClient class

7b8ece9

Use Empty instead of String for the task cancel message

a6de4eb

mkhansenbot reviewed Sep 5, 2018

View reviewed changes

Fix minor build problem - wrong name for dependency

9c7a307

crdelsey reviewed Sep 5, 2018

View reviewed changes

Address more review feedback

bf274ac

mkhansenbot approved these changes Sep 6, 2018

View reviewed changes

crdelsey approved these changes Sep 6, 2018

View reviewed changes

mjeronimo merged commit acd0a55 into master Sep 6, 2018

mkhansenbot mentioned this pull request Sep 7, 2018

Add waitForServer to TaskClient #45

Closed

mjeronimo deleted the ros2_devel branch October 10, 2018 16:58

ghost referenced this pull request in logivations/navigation2 Mar 7, 2022

Oscillating paths smac: increase motion primitives number (#38)

79255ad

* obstacle avoidance fine tune * revert bin number * add log that tells which motion primitives are used * change dx, dy * typo * finetune

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Merge development work to the master branch. #38

Merge development work to the master branch. #38

mjeronimo commented Aug 30, 2018

mkhansenbot left a comment

mkhansenbot left a comment •

edited by mjeronimo

mkhansenbot Aug 30, 2018

mkhansenbot Aug 30, 2018

mkhansenbot Aug 30, 2018

mkhansenbot Aug 30, 2018

mkhansenbot Aug 30, 2018

mjeronimo Aug 30, 2018 •

edited

mkhansenbot Aug 30, 2018

mjeronimo Aug 30, 2018 •

edited

mkhansenbot Aug 30, 2018

mjeronimo Aug 30, 2018

mkhansenbot Aug 30, 2018

mkhansenbot commented Aug 30, 2018

mkhansenbot Aug 31, 2018

mkhansenbot Aug 31, 2018

mkhansenbot Aug 31, 2018

mkhansenbot Aug 31, 2018

mkhansenbot Aug 31, 2018

mkhansenbot Aug 31, 2018

mkhansenbot Aug 31, 2018

mjeronimo Sep 4, 2018

mkhansenbot left a comment

crdelsey Sep 5, 2018

crdelsey Sep 5, 2018

crdelsey Sep 5, 2018

crdelsey Sep 5, 2018

crdelsey Sep 5, 2018

mkhansenbot left a comment

Merge development work to the master branch. #38

Merge development work to the master branch. #38

Conversation

mjeronimo commented Aug 30, 2018

mkhansenbot left a comment

Choose a reason for hiding this comment

mkhansenbot left a comment • edited by mjeronimo

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

mjeronimo Aug 30, 2018 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mjeronimo Aug 30, 2018 • edited

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

mkhansenbot commented Aug 30, 2018

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

mkhansenbot 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

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mkhansenbot left a comment

Choose a reason for hiding this comment

mkhansenbot left a comment •

edited by mjeronimo

mjeronimo Aug 30, 2018 •

edited

mjeronimo Aug 30, 2018 •

edited