From 169f13096fc169af481743c7060d032d21131e05 Mon Sep 17 00:00:00 2001 From: Valeriy Date: Mon, 12 Feb 2024 19:27:02 -0800 Subject: [PATCH] Fix minor spacing and formatting issues --- .../controller_configuration_tutorial.rst | 223 +++++++++--------- 1 file changed, 115 insertions(+), 108 deletions(-) diff --git a/doc/controller_configuration/controller_configuration_tutorial.rst b/doc/controller_configuration/controller_configuration_tutorial.rst index 5e9f7600e..587540e89 100644 --- a/doc/controller_configuration/controller_configuration_tutorial.rst +++ b/doc/controller_configuration/controller_configuration_tutorial.rst @@ -14,7 +14,7 @@ Stock ROS Controllers The `JointTrajectoryController `_ and `GripperActionController `_ from `ROS controllers `_ package are supported out of the box for simple usage scenarios, and can be easily configured by using the `MoveIt Setup Assistant <../setup_assistant/setup_assistant_tutorial.html>`_ (*MSA*) on the *Controllers* page. -These controllers are integrated with MoveIt by using existing `MoveIt Controller Handles `_ +These controllers are integrated with MoveIt by using existing `MoveIt Controller Handles `_: * Stock `JointTrajectoryController `_ is integrated through `Joint Trajectory Controller Handle `_. * Stock `GripperActionController `_ is integrated through `Gripper Controller Handle `_. @@ -224,27 +224,34 @@ In this case, a *Controller Handle* and a *Controller Handle Allocator* may need The following package dependencies are required for implementing controller handles and allocators: -* ``moveit_ros_control_interface`` - Provides base classes for controller handles and allocators -* ``pluginlib`` - Provides macros for exporting a class as a plugin, only needed to export the controller handle allocator +* ``moveit_ros_control_interface`` - Provides base classes for controller handles and allocators. +* ``pluginlib`` - Provides macros for exporting a class as a plugin, only needed to export the controller handle allocator. The ``actionlib`` package may also be needed for communicating with the ROS controller via an *Action Client* if it exposes an *Action Server*. The following headers declare the relevant classes and macros: * ``#include `` + * declares ``moveit_controller_manager::MoveItControllerHandle`` class * declares ``moveit_ros_control_interface::ControllerHandleAllocator`` class + * ``#include `` + * declares ``PLUGINLIB_EXPORT_CLASS`` macro for exporting plugins Two example *controller handle* implementations are included with MoveIt: * `follow_joint_trajectory_controller_handle.h `_ + * See implementation in `follow_joint_trajectory_controller_handle.cpp `_ + * `gripper_controller_handle.h `_ + * Implemented inline in the same header file As you can see, writing a `controller handle `_ comes down to implementing: + * ``sendTrajectory`` method that translates `moveit_msgs::RobotTrajectory `_ to a format the controller can understand * ``cancelExecution`` method to tell the controller to stop any active trajectories * ``waitForExecution`` method that will block the calling thread until the controller finishes or the ``timeout`` is reached @@ -258,25 +265,26 @@ The only job of a controller handle allocator is to create a new instance of the .. code-block:: c++ - #include "controller_handle_example.h" // the handle being allocated - #include - #include + // declares example::controller_handle_example class + #include "controller_handle_example.h" + #include + #include - namespace example - { - class controller_handle_allocator_example : public moveit_ros_control_interface::ControllerHandleAllocator - { - public: + namespace example + { + class controller_handle_allocator_example : public moveit_ros_control_interface::ControllerHandleAllocator + { + public: moveit_controller_manager::MoveItControllerHandlePtr alloc(const std::string& name, - const std::vector& resources) override + const std::vector& resources) override { - return std::make_shared(name, std::string("follow_joint_trajectory")); + return std::make_shared(name, std::string("follow_joint_trajectory")); } - }; - } // namespace example + }; + } // namespace example - PLUGINLIB_EXPORT_CLASS(example::controller_handle_allocator_example, - moveit_ros_control_interface::ControllerHandleAllocator); + PLUGINLIB_EXPORT_CLASS(example::controller_handle_allocator_example, + moveit_ros_control_interface::ControllerHandleAllocator); This example controller handle allocator can be exported by creating a plugin definition file which is then referenced in the ``exports`` section of ``package.xml``: @@ -315,101 +323,100 @@ The translation between `moveit_msgs::RobotTrajectory + #include + #include + #include + + namespace example + { + class controller_handle_example : public moveit_controller_manager::MoveItControllerHandle + { + private: + // Idle or done executing trajectory + bool done_; - #include - #include - #include - #include + // Connects to Action Server exposed by the controller + std::shared_ptr> actionClient_; + + public: + controller_handle_example(const std::string& name, const std::string& action_ns) + { + std::string actionName = name + "/" + action_ns; - namespace example + actionClient_ = + std::make_shared>(actionName, true); + + // Timeout can be loaded from settings + actionClient_->waitForServer(ros::Duration(20.0)); + + if (!actionClient_->isServerConnected()) { - class controller_handle_example : public moveit_controller_manager::MoveItControllerHandle + // Report connection error + actionClient_.reset(); + } + } + + public: + // MoveIt calls this method when it wants to send a trajectory goal to execute + bool sendTrajectory(const moveit_msgs::RobotTrajectory& trajectory) override + { + if (!actionClient_) { - private: - // Idle or done executing trajectory - bool done_; - - // Connects to Action Server exposed by the controller - std::shared_ptr> actionClient_; - - public: - controller_handle_example(const std::string& name, const std::string& action_ns) - { - std::string actionName = name + "/" + action_ns; - - actionClient_ = - std::make_shared>(actionName, true); - - actionClient_->waitForServer(ros::Duration(20.0)); - - if (!actionClient_->isServerConnected()) - { - // Report connection error - actionClient_.reset(); - } - } - - public: - // MoveIt calls this method when it wants to send a trajectory goal to execute - bool sendTrajectory(const moveit_msgs::RobotTrajectory& trajectory) override - { - if (!actionClient_) - { - // Report connection error - return false; - } - - control_msgs::FollowJointTrajectoryGoal goal; - goal.trajectory = trajectory.joint_trajectory; - - actionClient_->sendGoal( - goal, - [this](const auto& state, const auto& result) { - // Completed trajectory - done_ = true; - }, - [this] { - // Beginning trajectory - }, - [this](const auto& feedback) { - // Trajectory feedback - }); - - done_ = false; - - return true; - } - - // MoveIt calls this method when it wants a blocking call until done - bool waitForExecution(const ros::Duration& timeout = ros::Duration(0)) override - { - if (actionClient_ && !done_) - return actionClient_->waitForResult(ros::Duration(5.0)); - - return true; - } - - // MoveIt calls this method to get status updates - moveit_controller_manager::ExecutionStatus getLastExecutionStatus() override - { - // Report last status here - return moveit_controller_manager::ExecutionStatus::SUCCEEDED; - } - - // MoveIt calls this method to abort trajectory goal execution - bool cancelExecution() override - { - if (!actionClient_) - return false; - - actionClient_->cancelGoal(); - done_ = true; - - return true; - } - }; - } // namespace example + // Report connection error + return false; + } + + control_msgs::FollowJointTrajectoryGoal goal; + goal.trajectory = trajectory.joint_trajectory; + + actionClient_->sendGoal( + goal, + [this](const auto& state, const auto& result) { + // Complete trajectory callback + done_ = true; + }, + [this] { + // Begin trajectory callback + }, + [this](const auto& feedback) { + // Trajectory state callback + }); + + done_ = false; + + return true; + } + + // MoveIt calls this method when it wants a blocking call until done + bool waitForExecution(const ros::Duration& timeout = ros::Duration(0)) override + { + if (actionClient_ && !done_) + return actionClient_->waitForResult(ros::Duration(5.0)); + + return true; + } + + // MoveIt calls this method to get status updates + moveit_controller_manager::ExecutionStatus getLastExecutionStatus() override + { + // Report last status here + return moveit_controller_manager::ExecutionStatus::SUCCEEDED; + } + + // MoveIt calls this method to abort trajectory goal execution + bool cancelExecution() override + { + if (!actionClient_) + return false; + + actionClient_->cancelGoal(); + done_ = true; + + return true; + } + }; + } // namespace example .. note:: Replace ``your_controller_action`` with the type of action interface supported by the controller, and ``your_timeout`` with how long to wait for the connection to take place (this can be read from settings). If the controller doesn't support an Action Server, this can be replaced by whichever mechanism is supported.