#include <MultiDOFControllerManager.h>
Public Member Functions | |
virtual void | getActiveControllers (std::vector< std::string > &names) |
virtual moveit_controller_manager::MoveItControllerHandlePtr | getControllerHandle (const std::string &name) |
virtual void | getControllerJoints (const std::string &name, std::vector< std::string > &joints) |
virtual void | getControllersList (std::vector< std::string > &names) |
virtual moveit_controller_manager::MoveItControllerManager::ControllerState | getControllerState (const std::string &name) |
virtual void | getLoadedControllers (std::vector< std::string > &names) |
MultiDOFControllerManager () | |
virtual bool | switchControllers (const std::vector< std::string > &activate, const std::vector< std::string > &deactivate) |
virtual | ~MultiDOFControllerManager () |
Protected Attributes | |
std::map< std::string, ActionBasedControllerHandleBasePtr > | controllers_ |
ros::NodeHandle | node_handle_ |
Essentially a variation of MoveItSimpleControllerManager (moveit_plugins/moveit_simple_controller_manager) which supports only *one* type of controller: The "FollowRobotTrajectory" type.
The "FollowRobotTrajectory" controller does nothing but take the moveit_msgs/RobotTrajectory sent by MoveIt! and *forward* it to an implementation which follows the robot trajectory *including* taking into account navigation for the virtual joint, specified in moveit_msgs::RobotTrajectory.multi_dof_joint_trajectory.
The actual following of the moveit_msgs/RobotTrajectory is provided in the moveit_controller_multidof::ForwardingControllerHandle.
This implementation is essentially a copy of MoveItSimpleControllerManager (moveit_plugins/moveit_simple_controller_manager, with the only change that only the "FollowRobotTrajectory" type is supported. It would be nice to *extend* MoveItSimpleControllerManager to support the type of controller which has a concept of the virtual joint and therefore supports MulitDOF trajectories. However unfortunately at this stage the implementation of MoveItSimpleControllerManager is in a .cpp file and can't be extended. Some time in future this may be the case, and then this implementation should be changed accordingly. For now, the implementation is made to act just as an exteded version of MoveItSimpleControllerManager.
Accordingly, parameters in the .yaml config are to be in the following format:
``` virtual_joint_name: <name of="" virtual="" joint>=""> path_navigation_action_topic: /navigate_path joint_trajectory_action_topic: /jaco/joint_trajectory_action controller_list:
Note that the type "FollowRobotTrajectory" is new, and that "virtual_joint_name" can be specified. If it is not specified, then the robot is considered immobile, and only joint trajectory actions are supported. The topic for the joint trajectory action needs to be specified, and also the path navigation topic if robot is mobile. The list of joints must include all joints including the virtual joint. The parameter action_ns doesn't have to be specified as it is not used at the moment, but it should be declared because otherwise the parameters can't be read uniformly with the code of MoveItSimpleControllerManager.
*Note for developers*: All changes to the copy of MoveItSimpleControllerManager that have been made are in-between comments "MULTIDOF_CHANGE". This should make updating this implementation according to newer versions of MoveItSimpleControllerManager easier. Unfortunately at this stage, this is the easiest way to do this.
Definition at line 87 of file MultiDOFControllerManager.h.
Definition at line 16 of file MultiDOFControllerManager.cpp.
virtual moveit_controller_multidof::MultiDOFControllerManager::~MultiDOFControllerManager | ( | ) | [inline, virtual] |
Definition at line 92 of file MultiDOFControllerManager.h.
virtual void moveit_controller_multidof::MultiDOFControllerManager::getActiveControllers | ( | std::vector< std::string > & | names | ) | [inline, virtual] |
Implements moveit_controller_manager::MoveItControllerManager.
Definition at line 109 of file MultiDOFControllerManager.h.
moveit_controller_manager::MoveItControllerHandlePtr MultiDOFControllerManager::getControllerHandle | ( | const std::string & | name | ) | [virtual] |
Implements moveit_controller_manager::MoveItControllerManager.
Definition at line 193 of file MultiDOFControllerManager.cpp.
void MultiDOFControllerManager::getControllerJoints | ( | const std::string & | name, |
std::vector< std::string > & | joints | ||
) | [virtual] |
Implements moveit_controller_manager::MoveItControllerManager.
Definition at line 210 of file MultiDOFControllerManager.cpp.
void MultiDOFControllerManager::getControllersList | ( | std::vector< std::string > & | names | ) | [virtual] |
Implements moveit_controller_manager::MoveItControllerManager.
Definition at line 203 of file MultiDOFControllerManager.cpp.
moveit_controller_manager::MoveItControllerManager::ControllerState MultiDOFControllerManager::getControllerState | ( | const std::string & | name | ) | [virtual] |
Implements moveit_controller_manager::MoveItControllerManager.
Definition at line 225 of file MultiDOFControllerManager.cpp.
virtual void moveit_controller_multidof::MultiDOFControllerManager::getLoadedControllers | ( | std::vector< std::string > & | names | ) | [inline, virtual] |
Definition at line 117 of file MultiDOFControllerManager.h.
virtual bool moveit_controller_multidof::MultiDOFControllerManager::switchControllers | ( | const std::vector< std::string > & | activate, |
const std::vector< std::string > & | deactivate | ||
) | [inline, virtual] |
Implements moveit_controller_manager::MoveItControllerManager.
Definition at line 133 of file MultiDOFControllerManager.h.
std::map<std::string, ActionBasedControllerHandleBasePtr> moveit_controller_multidof::MultiDOFControllerManager::controllers_ [protected] |
Definition at line 138 of file MultiDOFControllerManager.h.
Definition at line 137 of file MultiDOFControllerManager.h.