diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index 3d12582654..2758f2099b 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -148,22 +148,22 @@ class MoveGroupInterface (which is the default type of callback queues used in ROS) \param tf_buffer. Specify a TF2_ROS Buffer instance to use. If not specified, one will be constructed internally along with an internal TF2_ROS TransformListener - \param wait_for_servers. Timeout for connecting to action servers. Zero time means unlimited waiting. + \param wait_for_servers. Timeout for connecting to action servers. -1 time means unlimited waiting. */ MoveGroupInterface(const rclcpp::Node::SharedPtr& node, const Options& opt, const std::shared_ptr& tf_buffer = std::shared_ptr(), - const rclcpp::Duration& wait_for_servers = rclcpp::Duration(0.0)); + const rclcpp::Duration& wait_for_servers = rclcpp::Duration(-1)); /** \brief Construct a client for the MoveGroup action for a particular \e group. \param tf_buffer. Specify a TF2_ROS Buffer instance to use. If not specified, one will be constructed internally along with an internal TF2_ROS TransformListener - \param wait_for_servers. Timeout for connecting to action servers. Zero time means unlimited waiting. + \param wait_for_servers. Timeout for connecting to action servers. -1 time means unlimited waiting. */ MoveGroupInterface(const rclcpp::Node::SharedPtr& node, const std::string& group, const std::shared_ptr& tf_buffer = std::shared_ptr(), - const rclcpp::Duration& wait_for_servers = rclcpp::Duration(0.0)); + const rclcpp::Duration& wait_for_servers = rclcpp::Duration(-1)); ~MoveGroupInterface(); diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 1db631a01c..0f5b82ed20 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -143,12 +143,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl current_state_monitor_ = getSharedStateMonitor(node_, robot_model_, tf_buffer_); - rclcpp::Time timeout_for_servers = pnode_->now() + wait_for_servers; - if (wait_for_servers == rclcpp::Duration(0.0)) - timeout_for_servers = rclcpp::Time(); // wait for ever - move_action_client_ = rclcpp_action::create_client(pnode_, move_group::MOVE_ACTION); - move_action_client_->wait_for_action_server(std::chrono::nanoseconds(timeout_for_servers.nanoseconds())); + move_action_client_->wait_for_action_server(wait_for_servers.to_chrono>()); // TODO(JafarAbdi): Enable once moveit_ros_manipulation is ported // pick_action_client_ = rclcpp_action::create_client( // node_, move_group::PICKUP_ACTION); @@ -159,7 +155,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl // place_action_client_->wait_for_action_server(std::chrono::nanoseconds(timeout_for_servers.nanoseconds())); execute_action_client_ = rclcpp_action::create_client(pnode_, move_group::EXECUTE_ACTION_NAME); - execute_action_client_->wait_for_action_server(std::chrono::nanoseconds(timeout_for_servers.nanoseconds())); + execute_action_client_->wait_for_action_server(wait_for_servers.to_chrono>()); query_service_ = pnode_->create_client(move_group::QUERY_PLANNERS_SERVICE_NAME);