From da312c52189e75eabb518bf9c49cfc7c3ad8167d Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 23 Jul 2024 17:32:55 -0700 Subject: [PATCH] Add new accessor methods to move group interface Signed-off-by: methylDragon --- .../move_group_interface.h | 14 +++++ .../src/move_group_interface.cpp | 55 +++++++++++++------ 2 files changed, 53 insertions(+), 16 deletions(-) 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 f3c7b6f506..65205457a6 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 @@ -236,6 +236,9 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface If the value is greater than 1, it is set to 1.0. */ void setMaxVelocityScalingFactor(double max_velocity_scaling_factor); + /** \brief Get the max velocity scaling factor set by setMaxVelocityScalingFactor(). */ + double getMaxVelocityScalingFactor() const; + /** \brief Set a scaling factor for optionally reducing the maximum joint acceleration. Allowed values are in (0,1]. The maximum joint acceleration specified in the robot model is multiplied by the factor. If the value is 0, it is set to @@ -243,6 +246,9 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface If the value is greater than 1, it is set to 1.0. */ void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor); + /** \brief Get the max acceleration scaling factor set by setMaxAccelerationScalingFactor(). */ + double getMaxAccelerationScalingFactor() const; + /** \brief Get the number of seconds set by setPlanningTime() */ double getPlanningTime() const; @@ -796,6 +802,14 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface /** \brief How often is the system allowed to move the camera to update environment model when looking */ void setLookAroundAttempts(int32_t attempts); + /** \brief Build a RobotState message for use with plan() or computeCartesianPath() + * If the move_group has a custom set start state, this method will use that as the robot state. + * + * Otherwise, the robot state will be with `is_diff` set to true, causing it to be an offset from the current state + * of the robot at time of the state's use. + */ + void constructRobotState(moveit_msgs::msg::RobotState& state); + /** \brief Build the MotionPlanRequest that would be sent to the move_group action with plan() or move() and store it in \e request */ void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request); 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 4f93f72781..028819a611 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 @@ -359,11 +359,21 @@ class MoveGroupInterface::MoveGroupInterfaceImpl setMaxScalingFactor(max_velocity_scaling_factor_, value, "velocity_scaling_factor", 0.1); } + double getMaxVelocityScalingFactor() const + { + return max_velocity_scaling_factor_; + } + void setMaxAccelerationScalingFactor(double value) { setMaxScalingFactor(max_acceleration_scaling_factor_, value, "acceleration_scaling_factor", 0.1); } + double getMaxAccelerationScalingFactor() const + { + return max_acceleration_scaling_factor_; + } + void setMaxScalingFactor(double& variable, const double target_value, const char* factor_name, double fallback_value) { if (target_value > 1.0) @@ -857,14 +867,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl auto req = std::make_shared(); moveit_msgs::srv::GetCartesianPath::Response::SharedPtr response; - if (considered_start_state_) - { - moveit::core::robotStateToRobotStateMsg(*considered_start_state_, req->start_state); - } - else - { - req->start_state.is_diff = true; - } + constructRobotState(req->start_state); req->group_name = opt_.group_name; req->header.frame_id = getPoseReferenceFrame(); @@ -1009,6 +1012,18 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return allowed_planning_time_; } + void constructRobotState(moveit_msgs::msg::RobotState& state) const + { + if (considered_start_state_) + { + moveit::core::robotStateToRobotStateMsg(*considered_start_state_, state); + } + else + { + state.is_diff = true; + } + } + void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request) const { request.group_name = opt_.group_name; @@ -1020,14 +1035,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl request.planner_id = planner_id_; request.workspace_parameters = workspace_parameters_; - if (considered_start_state_) - { - moveit::core::robotStateToRobotStateMsg(*considered_start_state_, request.start_state); - } - else - { - request.start_state.is_diff = true; - } + constructRobotState(request.start_state); if (active_target_ == JOINT) { @@ -1441,11 +1449,21 @@ void MoveGroupInterface::setMaxVelocityScalingFactor(double max_velocity_scaling impl_->setMaxVelocityScalingFactor(max_velocity_scaling_factor); } +double MoveGroupInterface::getMaxVelocityScalingFactor() const +{ + return impl_->getMaxVelocityScalingFactor(); +} + void MoveGroupInterface::setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor) { impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor); } +double MoveGroupInterface::getMaxAccelerationScalingFactor() const +{ + return impl_->getMaxAccelerationScalingFactor(); +} + moveit::core::MoveItErrorCode MoveGroupInterface::asyncMove() { return impl_->move(false); @@ -2326,6 +2344,11 @@ bool MoveGroupInterface::detachObject(const std::string& name) return impl_->detachObject(name); } +void MoveGroupInterface::constructRobotState(moveit_msgs::msg::RobotState& state) +{ + impl_->constructRobotState(state); +} + void MoveGroupInterface::constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& goal_out) { impl_->constructMotionPlanRequest(goal_out);