Spin the node from the blackboard before calling getCurrentPose

[mjeronimo]

Description

• The node on the blackboard is not on an executor. Before calling getRobotPose, we must spin_some so that it processes incoming messages.

[crdelsey]

Is there a way to just spin the node in a background thread, so we don't need to manually spin every time we want to take an action?

[SteveMacenski]

@crdelsey that's generally how the main of ROS1 things were generally structured.

int main()
{
  ros::init(stuff);
  ros::NodeHandle nh("scope");
  ProgramClass obj(nh);
  ros::spin();
}

[mjeronimo]

@crdelsey @SteveMacenski Yes, we could put a spinning node on the blackboard, but we'd also need a non-spinning one for service calls. Would you prefer this? This would be similar to what I have in the lifecycle nodes, where there is an rclcpp_node (a spinning one) and a client_node (non-spinning).

[crdelsey]

we'd also need a non-spinning one for service calls

Does the problem come from the spin_until_future_complete call? Maybe the service interface needs an equivalent wait_until_future_complete for an already spinning node. Futures should be able to do something like this automatically.

[mjeronimo]

Yes, the main issue is that a node used with an executor can't already be on another executor. spin_until_future_complete, used in service calls, puts it on an executor (spin_node_until_future_complete is called by spin_until_future_complete):

template<typename ResponseT, typename TimeRepT = int64_t, typename TimeT = std::milli>
rclcpp::executor::FutureReturnCode
spin_node_until_future_complete(
  rclcpp::executor::Executor & executor,
  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
  std::shared_future<ResponseT> & future,
  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
{ 
  // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
  // inside a callback executed by an executor.
  executor.add_node(node_ptr);
  auto retcode = executor.spin_until_future_complete(future, timeout);
  executor.remove_node(node_ptr);
  return retcode;
}

That's a great idea about using the future's wait_util. As a test, I did a quick hack of the minimal_client and got this to work:

void spin_node(rclcpp::Node::SharedPtr node)
{
   rclcpp::spin(node);
}

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("minimal_client");
  auto client = node->create_client<AddTwoInts>("add_two_ints");

  std::thread t(spin_node, node);

  while (!client->wait_for_service(std::chrono::seconds(1))) {
    if (!rclcpp::ok()) {
      RCLCPP_ERROR(node->get_logger(), "client interrupted while waiting for service to appear.");
      return 1;
    }
    RCLCPP_INFO(node->get_logger(), "waiting for service to appear...");
  }

  auto request = std::make_shared<AddTwoInts::Request>();
  request->a = 41;
  request->b = 1;
  auto result_future = client->async_send_request(request);

  std::chrono::system_clock::time_point two_seconds_passed
        = std::chrono::system_clock::now() + std::chrono::seconds(2);
  auto status = result_future.wait_until(two_seconds_passed);

  printf("status: %d\n", (int) status);

#if 0
  if (rclcpp::spin_until_future_complete(node, result_future) !=
    rclcpp::executor::FutureReturnCode::SUCCESS)
  {
    RCLCPP_ERROR(node->get_logger(), "service call failed :(");
    return 1;
  }
#endif

  auto result = result_future.get();
  RCLCPP_INFO(node->get_logger(), "result of %" PRId64 " + %" PRId64 " = %" PRId64,
    request->a, request->b, result->sum);
  rclcpp::shutdown();

  t.join();

  return 0;
}

If we want to take this approach, we can eliminate several "client_nodes" that we're holding just for service calls. Perhaps we could integrate this PR to make sure that IsLocalized and GoalReached work and then start a new PR to get rid of all service client (non-spinning) nodes.

[mjeronimo]

By the way, here is the node list for the system running in simulation. I count 8 nodes that can be eliminated with this method. Next, after this, we can focus on eliminating the rclcpp_nodes, which we've been using to bridge to ROS2 functionality that doesn't yet support lifecycle nodes.

/amcl
/amcl_map_client
/amcl_rclcpp_node
/bt_navigator
/bt_navigator_client_node
/bt_navigator_global_localization_client
/bt_navigator_rclcpp_node
/dwb_controller
/dwb_controller_rclcpp_node
/gazebo
/global_costmap_client
/global_costmap/global_costmap
/global_costmap/global_costmap_rclcpp_node
/launch_ros
/lifecycle_manager
/lifecycle_manager_client_service_client
/lifecycle_manager_service_client
/local_costmap_client
/local_costmap/local_costmap
/local_costmap/local_costmap_rclcpp_node
/map_server
/navfn_planner
/navfn_planner_GetCostmap_client
/navfn_planner_rclcpp_node
/navigation_dialog_action_client
/robot_state_publisher
/rviz
/turtlebot3_diff_drive
/turtlebot3_imu
/turtlebot3_joint_state
/turtlebot3_laserscan
/world_model

[SteveMacenski]

Maybe a dumb question -- but why not just use 1 node with a multithreaded executor? That's how I would go about something like this in ROS1 as well. One node spinning but spins up multiple threads so you can have many callbacks executed simultaneously. ROS2 documentation says it supports as well.

[mjeronimo]

@SteveMacenski Yes, I agree: I would like to begin to consolidate the nodes using compostion and not have so many separate processes. It's just on the TODO list. We could bring the nodes together into a single main with a multi-threaded executor. We'd be able to use the intra process communication as well, which should avoid the overhead of marshaling and unmarshaling messages. I think this should be a high priority.