diff --git a/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt b/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt index 9905ae3238..ae54b68b96 100644 --- a/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt +++ b/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt @@ -11,12 +11,18 @@ find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(control_msgs REQUIRED) find_package(rclcpp_action REQUIRED) +find_package(generate_parameter_library REQUIRED) # Finds Boost Components include(ConfigExtras.cmake) set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs rclcpp rclcpp_action moveit_core - pluginlib) + pluginlib generate_parameter_library) + +generate_parameter_library( + moveit_simple_controller_manager_parameters # cmake target name for the parameter library + src/moveit_simple_controller_manager_parameters.yaml # path to input yaml file +) include_directories(include) @@ -32,6 +38,16 @@ set_target_properties( ament_target_dependencies(moveit_simple_controller_manager ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) +target_link_libraries(moveit_simple_controller_manager moveit_simple_controller_manager_parameters) + +install( + TARGETS moveit_simple_controller_manager_parameters + EXPORT ${PROJECT_NAME}Targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + install( TARGETS moveit_simple_controller_manager EXPORT moveit_simple_controller_managerTargets diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h index 70ae825680..43f10fb551 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h @@ -63,6 +63,11 @@ class FollowJointTrajectoryControllerHandle // TODO(JafarAbdi): Revise parameter lookup // void configure(XmlRpc::XmlRpcValue& config) override; + void setPathTolerance(double path_tolerance); + + void setGoalTolerance(double goal_tolerance); + + void setGoalTimeTolerance(double goal_time_tolerance); protected: static control_msgs::msg::JointTolerance& getTolerance(std::vector& tolerances, diff --git a/moveit_plugins/moveit_simple_controller_manager/package.xml b/moveit_plugins/moveit_simple_controller_manager/package.xml index e80e46f01c..8939df5099 100644 --- a/moveit_plugins/moveit_simple_controller_manager/package.xml +++ b/moveit_plugins/moveit_simple_controller_manager/package.xml @@ -25,6 +25,7 @@ pluginlib control_msgs rclcpp_action + generate_parameter_library ament_cmake diff --git a/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp b/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp index c7b49e0751..dacfc37634 100644 --- a/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp +++ b/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp @@ -36,6 +36,7 @@ /* Author: Michael Ferguson, Ioan Sucan, E. Gil Jones */ #include +#include using namespace std::placeholders; @@ -97,6 +98,19 @@ bool FollowJointTrajectoryControllerHandle::sendTrajectory(const moveit_msgs::ms return true; } +void FollowJointTrajectoryControllerHandle::setPathTolerance(double /*path_tolerance*/) +{ +} + +void FollowJointTrajectoryControllerHandle::setGoalTolerance(double /*goal_tolerance*/) +{ +} + +void FollowJointTrajectoryControllerHandle::setGoalTimeTolerance(double goal_time_tolerance) +{ + goal_template_.goal_time_tolerance = rclcpp::Duration::from_seconds(goal_time_tolerance); +} + // TODO(JafarAbdi): Revise parameter lookup // void FollowJointTrajectoryControllerHandle::configure(XmlRpc::XmlRpcValue& config) //{ diff --git a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp index 4d4cedb8f7..560b176f82 100644 --- a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp +++ b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp @@ -49,6 +49,8 @@ #include #include +#include + namespace { /** @@ -101,62 +103,33 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo void initialize(const rclcpp::Node::SharedPtr& node) override { node_ = node; - if (!node_->has_parameter(makeParameterName(PARAM_BASE_NAME, "controller_names"))) - { - RCLCPP_ERROR_STREAM(getLogger(), "No controller_names specified."); - return; - } - rclcpp::Parameter controller_names_param; - node_->get_parameter(makeParameterName(PARAM_BASE_NAME, "controller_names"), controller_names_param); - if (controller_names_param.get_type() != rclcpp::ParameterType::PARAMETER_STRING_ARRAY) + try { - RCLCPP_ERROR(getLogger(), "Parameter controller_names should be specified as a string array"); - return; - } - std::vector controller_names = controller_names_param.as_string_array(); - /* actually create each controller */ - for (const std::string& controller_name : controller_names) - { - try - { - std::string action_ns; - const std::string& action_ns_param = makeParameterName(PARAM_BASE_NAME, controller_name, "action_ns"); - if (!node_->get_parameter(action_ns_param, action_ns)) - { - RCLCPP_ERROR_STREAM(getLogger(), "No action namespace specified for controller `" - << controller_name << "` through parameter `" << action_ns_param << '`'); - continue; - } + const auto controller_manager_param_prefix = "moveit_simple_controller_manager"; + param_listener_ = + std::make_shared(node_, controller_manager_param_prefix); + params_ = param_listener_->get_params(); - std::string type; - if (!node_->get_parameter(makeParameterName(PARAM_BASE_NAME, controller_name, "type"), type)) - { - RCLCPP_ERROR_STREAM(getLogger(), "No type specified for controller " << controller_name); - continue; - } + std::vector controller_names = params_.controller_names; - std::vector controller_joints; - if (!node_->get_parameter(makeParameterName(PARAM_BASE_NAME, controller_name, "joints"), controller_joints) || - controller_joints.empty()) - { - RCLCPP_ERROR_STREAM(getLogger(), "No joints specified for controller " << controller_name); - continue; - } + /* actually create each controller */ + for (const auto& controller : params_.controller_names_map) + { + const std::string controller_name = controller.first; + const std::string action_ns = controller.second.action_ns; + const std::string controller_type = controller.second.type; + const std::vector controller_joints = controller.second.joints; ActionBasedControllerHandleBasePtr new_handle; - if (type == "GripperCommand") + if (controller_type == "GripperCommand") { - double max_effort; - const std::string& max_effort_param = makeParameterName(PARAM_BASE_NAME, controller_name, "max_effort"); - if (!node->get_parameter(max_effort_param, max_effort)) - { - RCLCPP_INFO_STREAM(getLogger(), "Max effort set to 0.0"); - max_effort = 0.0; - } + const double max_effort = controller.second.max_effort; + RCLCPP_INFO_STREAM(LOGGER, "Max effort set to " << max_effort); new_handle = std::make_shared(node_, controller_name, action_ns, max_effort); - bool parallel_gripper = false; - if (node_->get_parameter(makeParameterName(PARAM_BASE_NAME, "parallel"), parallel_gripper) && parallel_gripper) + + bool parallel_gripper = controller.second.parallel; + if (parallel_gripper) { if (controller_joints.size() != 2) { @@ -169,23 +142,28 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo } else { - std::string command_joint; - if (!node_->get_parameter(makeParameterName(PARAM_BASE_NAME, "command_joint"), command_joint)) + std::string command_joint = controller.second.command_joint; + if (command_joint.empty()) + { command_joint = controller_joints[0]; - + } static_cast(new_handle.get())->setCommandJoint(command_joint); } - bool allow_failure; - node_->get_parameter_or(makeParameterName(PARAM_BASE_NAME, "allow_failure"), allow_failure, false); + const bool allow_failure = controller.second.allow_failure; static_cast(new_handle.get())->allowFailure(allow_failure); RCLCPP_INFO_STREAM(getLogger(), "Added GripperCommand controller for " << controller_name); controllers_[controller_name] = new_handle; } - else if (type == "FollowJointTrajectory") + else if (controller_type == "FollowJointTrajectory") { new_handle = std::make_shared(node_, controller_name, action_ns); + + // static_cast(new_handle.get())->setPathTolerance(controller.second.path_tolerance); + // static_cast(new_handle.get())->setGoalTolerance(controller.second.goal_tolerance); + static_cast(new_handle.get()) + ->setGoalTimeTolerance(controller.second.goal_time_tolerance); RCLCPP_INFO_STREAM(getLogger(), "Added FollowJointTrajectory controller for " << controller_name); controllers_[controller_name] = new_handle; } @@ -201,8 +179,7 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo } moveit_controller_manager::MoveItControllerManager::ControllerState state; - node_->get_parameter_or(makeParameterName(PARAM_BASE_NAME, controller_name, "default"), state.default_, false); - state.active_ = true; + state.active_ = controller.second.make_default; controller_states_[controller_name] = state; @@ -216,6 +193,7 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo } } } + /* * Get a controller, by controller name (which was specified in the controllers.yaml */ @@ -298,6 +276,9 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo rclcpp::Node::SharedPtr node_; std::map controllers_; std::map controller_states_; + + std::shared_ptr param_listener_; + moveit_simple_controller_manager::Params params_; }; } // end namespace moveit_simple_controller_manager diff --git a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager_parameters.yaml b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager_parameters.yaml new file mode 100644 index 0000000000..68dfbd595b --- /dev/null +++ b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager_parameters.yaml @@ -0,0 +1,103 @@ +moveit_simple_controller_manager: + controller_names: { + type: string_array, + default_value: [], + description: "List of controllers for MoveIt's controller manager", + validation: { + not_empty<>: [] + } + } + __map_controller_names: + action_ns: { + type: string, + default_value: "", + description: "Action namespace for the controller. The action topic names will start with `/controller_name/action_ns`", + validation: { + not_empty<>: [] + } + } + type: { + type: string, + default_value: "", + description: "Controller type. FollowJointTrajectory or GripperCommand", + validation: { + not_empty<>: [] + } + } + make_default: { + type: bool, + default_value: false, + description: "This param was originally named default. Marking a controller as + default makes MoveIt prefer this controller when multiple options are available.", + validation: { + not_empty<>: [] + } + } + joints: { + type: string_array, + default_value: [], + description: "list of joints to actuate", + validation: { + not_empty<>: [] + } + } + # parameters for gripper controllers + max_effort: { + type: double, + default_value: 0.0, + description: "Max Effort. Parameter to be set if using GripperCommand" + } + parallel: { + type: bool, + default_value: false, + description: "Parallel gripper require exactly two joints. If true, specify only two joints. Parameter to be set if using GripperCommand" + } + command_joint: { + type: string, + default_value: "", + description: "If not parallel, set the joint to command. Parameter to be set if using GripperCommand" + } + allow_failure: { + type: bool, + default_value: false, + description: "Parameter to be set if using GripperCommand" + } + # parameters for follow joint trajectory controllers + goal_time_tolerance: { + type: double, + default_value: 0.0, + description: "Parameter to be set if using FollowJointTrajectory" + } + # __map_joints: + # path_tolerance: + # position: { + # type: double, + # default_value: 0.0, + # description: "Parameter to be set if using FollowJointTrajectory" + # } + # velocity: { + # type: double, + # default_value: 0.0, + # description: "Parameter to be set if using FollowJointTrajectory" + # } + # acceleration: { + # type: double, + # default_value: 0.0, + # description: "Parameter to be set if using FollowJointTrajectory" + # } + # goal_tolerance: + # position: { + # type: double, + # default_value: 0.0, + # description: "Parameter to be set if using FollowJointTrajectory" + # } + # velocity: { + # type: double, + # default_value: 0.0, + # description: "Parameter to be set if using FollowJointTrajectory" + # } + # acceleration: { + # type: double, + # default_value: 0.0, + # description: "Parameter to be set if using FollowJointTrajectory" + # }