diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 071a4b39ae..04671ccf1e 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -136,7 +136,7 @@ jobs: path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml - name: Generate codecov report uses: rhaschke/lcov-action@main - if: matrix.env.CCOV && steps.ici.outputs.target_test_results == '0' + if: always() && matrix.env.CCOV && steps.ici.outputs.target_test_results == '0' with: docker: $DOCKER_IMAGE workdir: ${{ env.BASEDIR }}/target_ws @@ -153,7 +153,7 @@ jobs: name: clang-tidy path: ${{ env.BASEDIR }}/target_ws/src/$(basename $(pwd)) - name: Prepare target_ws for cache - if: always() && ! matrix.env.CCOV + if: always() && !matrix.env.CCOV run: | du -sh ${{ env.BASEDIR }}/target_ws sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete diff --git a/moveit_commander/src/moveit_commander/conversions.py b/moveit_commander/src/moveit_commander/conversions.py index dcf884aa47..1fd154104c 100644 --- a/moveit_commander/src/moveit_commander/conversions.py +++ b/moveit_commander/src/moveit_commander/conversions.py @@ -39,7 +39,7 @@ # Use Python 3.x behaviour as fallback and choose the non-unicode version from io import BytesIO as StringIO -from moveit_commander import MoveItCommanderException +from .exception import MoveItCommanderException from geometry_msgs.msg import Pose, PoseStamped, Transform import rospy import tf diff --git a/moveit_commander/src/moveit_commander/robot.py b/moveit_commander/src/moveit_commander/robot.py index ef8f096674..55f6e7b07c 100644 --- a/moveit_commander/src/moveit_commander/robot.py +++ b/moveit_commander/src/moveit_commander/robot.py @@ -32,7 +32,8 @@ # # Author: Ioan Sucan -from moveit_commander import MoveGroupCommander, MoveItCommanderException +from moveit_commander import MoveGroupCommander +from .exception import MoveItCommanderException from moveit_ros_planning_interface import _moveit_robot_interface from moveit_msgs.msg import RobotState from visualization_msgs.msg import MarkerArray diff --git a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h index cd596a8a62..d1ca3090d4 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h +++ b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h @@ -60,10 +60,12 @@ class AttachedBody /** \brief Construct an attached body for a specified \e link. * * The name of this body is \e id and it consists of \e shapes that attach to the link by the transforms - * \e shape_poses. The set of links that are allowed to be touched by this object is specified by \e touch_links. */ - AttachedBody(const LinkModel* link, const std::string& id, const Eigen::Isometry3d& pose, + * \e shape_poses. The set of links that are allowed to be touched by this object is specified by \e touch_links. + * detach_posture may describe a detach motion for the gripper when placing the object. + * The shape and subframe poses are relative to the \e pose, and \e pose is relative to the parent link. */ + AttachedBody(const LinkModel* parent, const std::string& id, const Eigen::Isometry3d& pose, const std::vector& shapes, const EigenSTL::vector_Isometry3d& shape_poses, - const std::set& touch_links, const trajectory_msgs::msg::JointTrajectory& attach_posture, + const std::set& touch_links, const trajectory_msgs::msg::JointTrajectory& detach_posture, const moveit::core::FixedTransformsMap& subframe_poses = moveit::core::FixedTransformsMap()); ~AttachedBody(); diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index bf015f8675..5a60d2821e 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -1312,6 +1312,13 @@ class RobotState /** \brief Update the state after setting a particular link to the input global transform pose.*/ void updateStateWithLinkAt(const LinkModel* link, const Eigen::Isometry3d& transform, bool backward = false); + /** \brief Get the latest link upwards the kinematic tree which is only connected via fixed joints. + * + * This behaves the same as RobotModel::getRigidlyConnectedParentLinkModel, + * but can additionally resolve parents for attached objects / subframes. + */ + const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const std::string& frame) const; + /** \brief Get the link transform w.r.t. the root link (model frame) of the RobotModel. * This is typically the root link of the URDF unless a virtual joint is present. * Checks the cache and if there are any dirty (non-updated) transforms, first updates them as needed. diff --git a/moveit_core/robot_state/src/attached_body.cpp b/moveit_core/robot_state/src/attached_body.cpp index 9226e702ec..027bfcd7be 100644 --- a/moveit_core/robot_state/src/attached_body.cpp +++ b/moveit_core/robot_state/src/attached_body.cpp @@ -43,12 +43,12 @@ namespace moveit { namespace core { -AttachedBody::AttachedBody(const LinkModel* parent_link_model, const std::string& id, const Eigen::Isometry3d& pose, +AttachedBody::AttachedBody(const LinkModel* parent, const std::string& id, const Eigen::Isometry3d& pose, const std::vector& shapes, const EigenSTL::vector_Isometry3d& shape_poses, const std::set& touch_links, const trajectory_msgs::msg::JointTrajectory& detach_posture, const FixedTransformsMap& subframe_poses) - : parent_link_model_(parent_link_model) + : parent_link_model_(parent) , id_(id) , pose_(pose) , shapes_(shapes) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index a521891256..79606d55dc 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -45,7 +45,6 @@ #else #include #endif - #include #include #include @@ -799,6 +798,31 @@ void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Isome it->second->computeTransform(global_link_transforms_[it->second->getAttachedLink()->getLinkIndex()]); } +const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::string& frame) const +{ + const moveit::core::LinkModel* link{ nullptr }; + + size_t idx = 0; + if ((idx = frame.find('/')) != std::string::npos) + { // resolve sub frame + std::string object{ frame.substr(0, idx) }; + if (!hasAttachedBody(object)) + return nullptr; + auto body{ getAttachedBody(object) }; + if (!body->hasSubframeTransform(frame)) + return nullptr; + link = body->getAttachedLink(); + } + else if (hasAttachedBody(frame)) + { + link = getAttachedBody(frame)->getAttachedLink(); + } + else if (getRobotModel()->hasLinkModel(frame)) + link = getLinkModel(frame); + + return getRobotModel()->getRigidlyConnectedParentLinkModel(link); +} + bool RobotState::satisfiesBounds(double margin) const { const std::vector& jm = robot_model_->getActiveJointModels(); diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index 01adb611b4..1421b98cdf 100644 --- a/moveit_core/robot_state/test/robot_state_test.cpp +++ b/moveit_core/robot_state/test/robot_state_test.cpp @@ -685,6 +685,39 @@ TEST_F(OneRobot, testInterpolation) EXPECT_TRUE(nan_exception) << "NaN interpolation parameter did not create expected exception."; } +TEST_F(OneRobot, rigidlyConnectedParent) +{ + // link_e is its own rigidly-connected parent + const moveit::core::LinkModel* link_e{ robot_model_->getLinkModel("link_e") }; + EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_e), link_e); + + // link_b is rigidly connected to its parent link_a + const moveit::core::LinkModel* link_a{ robot_model_->getLinkModel("link_a") }; + const moveit::core::LinkModel* link_b{ robot_model_->getLinkModel("link_b") }; + EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_b), link_a); + + moveit::core::RobotState state(robot_model_); + + EXPECT_EQ(state.getRigidlyConnectedParentLinkModel("link_b"), link_a); + + // attach "object" with "subframe" to link_b + state.attachBody(new moveit::core::AttachedBody( + link_b, "object", Eigen::Isometry3d::Identity(), std::vector{}, + EigenSTL::vector_Isometry3d{}, std::set{}, trajectory_msgs::msg::JointTrajectory{}, + moveit::core::FixedTransformsMap{ { "subframe", Eigen::Isometry3d::Identity() } })); + + // RobotState's version should resolve these too + EXPECT_EQ(link_a, state.getRigidlyConnectedParentLinkModel("object")); + EXPECT_EQ(link_a, state.getRigidlyConnectedParentLinkModel("object/subframe")); + + // test failure cases + EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("no_object")); + EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("object/no_subframe")); + EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("")); + EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("object/")); + EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("/")); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index 18510770cb..44defe4684 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -935,17 +935,27 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT max_velocity[j] = 1.0; if (bounds.velocity_bounded_) { + if (bounds.max_velocity_ < std::numeric_limits::epsilon()) + { + RCLCPP_ERROR(LOGGER, "Invalid max_velocity %f specified for '%s', must be greater than 0.0", + bounds.max_velocity_, vars[j].c_str()); + return false; + } max_velocity[j] = std::min(std::fabs(bounds.max_velocity_), std::fabs(bounds.min_velocity_)) * velocity_scaling_factor; - max_velocity[j] = std::max(0.01, max_velocity[j]); } max_acceleration[j] = 1.0; if (bounds.acceleration_bounded_) { + if (bounds.max_acceleration_ < std::numeric_limits::epsilon()) + { + RCLCPP_ERROR(LOGGER, "Invalid max_acceleration %f specified for '%s', must be greater than 0.0", + bounds.max_acceleration_, vars[j].c_str()); + return false; + } max_acceleration[j] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_)) * acceleration_scaling_factor; - max_acceleration[j] = std::max(0.01, max_acceleration[j]); } } diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h index e9e711dacb..99faacd8c4 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h @@ -63,7 +63,7 @@ class PlanningContextBase : public planning_interface::PlanningContext , terminated_(false) , model_(model) , limits_(limits) - , generator_(model, limits_) + , generator_(model, limits_, group) { } diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h index 769e1a35b3..194e157782 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h @@ -81,7 +81,8 @@ class TrajectoryGeneratorCIRC : public TrajectoryGenerator * */ TrajectoryGeneratorCIRC(const moveit::core::RobotModelConstPtr& robot_model, - const pilz_industrial_motion_planner::LimitsContainer& planner_limits); + const pilz_industrial_motion_planner::LimitsContainer& planner_limits, + const std::string& group_name); private: void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const override; diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h index 2b49ee87be..93834ee844 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h @@ -66,7 +66,8 @@ class TrajectoryGeneratorLIN : public TrajectoryGenerator * @param planner_limits: limits in joint and Cartesian spaces */ TrajectoryGeneratorLIN(const moveit::core::RobotModelConstPtr& robot_model, - const pilz_industrial_motion_planner::LimitsContainer& planner_limits); + const pilz_industrial_motion_planner::LimitsContainer& planner_limits, + const std::string& group_name); private: void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h index 53736e7ae4..0735394b39 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h @@ -61,7 +61,8 @@ class TrajectoryGeneratorPTP : public TrajectoryGenerator * @param model: a map of joint limits information */ TrajectoryGeneratorPTP(const moveit::core::RobotModelConstPtr& robot_model, - const pilz_industrial_motion_planner::LimitsContainer& planner_limits); + const pilz_industrial_motion_planner::LimitsContainer& planner_limits, + const std::string& group_name); private: void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, @@ -78,9 +79,8 @@ class TrajectoryGeneratorPTP : public TrajectoryGenerator * @param sampling_time */ void planPTP(const std::map& start_pos, const std::map& goal_pos, - trajectory_msgs::msg::JointTrajectory& joint_trajectory, const std::string& group_name, - const double& velocity_scaling_factor, const double& acceleration_scaling_factor, - const double& sampling_time); + trajectory_msgs::msg::JointTrajectory& joint_trajectory, const double& velocity_scaling_factor, + const double& acceleration_scaling_factor, const double& sampling_time); void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, const double& sampling_time, @@ -89,8 +89,8 @@ class TrajectoryGeneratorPTP : public TrajectoryGenerator private: const double MIN_MOVEMENT = 0.001; pilz_industrial_motion_planner::JointLimitsContainer joint_limits_; - // most strict joint limits for each group - std::map most_strict_limits_; + // most strict joint limits + JointLimit most_strict_limit_; }; } // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp index 8f85f6b753..4554b0ee36 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp @@ -116,7 +116,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updatePositionLimitF { // LCOV_EXCL_START case 0: - RCLCPP_ERROR_STREAM(LOGGER, "no bounds set for joint " << joint_model->getName()); + RCLCPP_WARN_STREAM(LOGGER, "no bounds set for joint " << joint_model->getName()); break; // LCOV_EXCL_STOP case 1: @@ -126,7 +126,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updatePositionLimitF break; // LCOV_EXCL_START default: - RCLCPP_ERROR_STREAM(LOGGER, "Multi-DOF-Joints not supported. The robot won't move. " << joint_model->getName()); + RCLCPP_WARN_STREAM(LOGGER, "Multi-DOF-Joint '" << joint_model->getName() << "' not supported."); joint_limit.has_position_limits = true; joint_limit.min_position = 0; joint_limit.max_position = 0; @@ -145,7 +145,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updateVelocityLimitF { // LCOV_EXCL_START case 0: - RCLCPP_ERROR_STREAM(LOGGER, "no bounds set for joint " << joint_model->getName()); + RCLCPP_WARN_STREAM(LOGGER, "no bounds set for joint " << joint_model->getName()); break; // LCOV_EXCL_STOP case 1: @@ -154,7 +154,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updateVelocityLimitF break; // LCOV_EXCL_START default: - RCLCPP_ERROR_STREAM(LOGGER, "Multi-DOF-Joints not supported. The robot won't move."); + RCLCPP_WARN_STREAM(LOGGER, "Multi-DOF-Joint '" << joint_model->getName() << "' not supported."); joint_limit.has_velocity_limits = true; joint_limit.max_velocity = 0; break; diff --git a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp index 940c8b8eaf..798aaff123 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp @@ -133,8 +133,8 @@ CommandPlanner::getPlanningContext(const planning_scene::PlanningSceneConstPtr& // Check that a loaded for this request exists if (!canServiceRequest(req)) { - RCLCPP_ERROR_STREAM(LOGGER, "No ContextLoader for planner_id " << req.planner_id.c_str() - << " found. Planning not possible."); + RCLCPP_ERROR_STREAM(LOGGER, "No ContextLoader for planner_id '" << req.planner_id.c_str() + << "' found. Planning not possible."); return nullptr; } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp index f5f6e00a29..d911ce2b60 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp @@ -59,7 +59,8 @@ namespace pilz_industrial_motion_planner static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator_circ"); TrajectoryGeneratorCIRC::TrajectoryGeneratorCIRC(const moveit::core::RobotModelConstPtr& robot_model, - const LimitsContainer& planner_limits) + const LimitsContainer& planner_limits, + const std::string& /*group_name*/) : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) { if (!planner_limits_.hasFullCartesianLimits()) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp index d483ae4d3e..1693e4dd7b 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -60,7 +60,7 @@ namespace pilz_industrial_motion_planner static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator_lin"); TrajectoryGeneratorLIN::TrajectoryGeneratorLIN(const moveit::core::RobotModelConstPtr& robot_model, - const LimitsContainer& planner_limits) + const LimitsContainer& planner_limits, const std::string& /*group_name*/) : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) { if (!planner_limits_.hasFullCartesianLimits()) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp index dd90feadc4..c65ed801a3 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp @@ -55,7 +55,7 @@ namespace pilz_industrial_motion_planner static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator_ptp"); TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const moveit::core::RobotModelConstPtr& robot_model, - const LimitsContainer& planner_limits) + const LimitsContainer& planner_limits, const std::string& group_name) : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) { if (!planner_limits_.hasJointLimits()) @@ -66,35 +66,29 @@ TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const moveit::core::RobotModelCon joint_limits_ = planner_limits_.getJointLimitContainer(); // collect most strict joint limits for each group in robot model - for (const auto& jmg : robot_model->getJointModelGroups()) - { - const auto& active_joints = jmg->getActiveJointModelNames(); + const auto* jmg = robot_model->getJointModelGroup(group_name); + if (!jmg) + throw TrajectoryGeneratorInvalidLimitsException("invalid group: " + group_name); - // no active joints - if (active_joints.empty()) - { - continue; - } + const auto& active_joints = jmg->getActiveJointModelNames(); - pilz_industrial_motion_planner::JointLimit most_strict_limit = joint_limits_.getCommonLimit(active_joints); + // no active joints + if (!active_joints.empty()) + { + most_strict_limit_ = joint_limits_.getCommonLimit(active_joints); - if (!most_strict_limit.has_velocity_limits) + if (!most_strict_limit_.has_velocity_limits) { - RCLCPP_ERROR_STREAM(LOGGER, "velocity limit not set for group " << jmg->getName()); - throw TrajectoryGeneratorInvalidLimitsException("velocity limit not set for group " + jmg->getName()); + throw TrajectoryGeneratorInvalidLimitsException("velocity limit not set for group " + group_name); } - if (!most_strict_limit.has_acceleration_limits) + if (!most_strict_limit_.has_acceleration_limits) { - RCLCPP_ERROR_STREAM(LOGGER, "acceleration limit not set for group " << jmg->getName()); - throw TrajectoryGeneratorInvalidLimitsException("acceleration limit not set for group " + jmg->getName()); + throw TrajectoryGeneratorInvalidLimitsException("acceleration limit not set for group " + group_name); } - if (!most_strict_limit.has_deceleration_limits) + if (!most_strict_limit_.has_deceleration_limits) { - RCLCPP_ERROR_STREAM(LOGGER, "deceleration limit not set for group " << jmg->getName()); - throw TrajectoryGeneratorInvalidLimitsException("deceleration limit not set for group " + jmg->getName()); + throw TrajectoryGeneratorInvalidLimitsException("deceleration limit not set for group " + group_name); } - - most_strict_limits_.insert(std::pair(jmg->getName(), most_strict_limit)); } RCLCPP_INFO(LOGGER, "Initialized Point-to-Point Trajectory Generator."); @@ -103,8 +97,8 @@ TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const moveit::core::RobotModelCon void TrajectoryGeneratorPTP::planPTP(const std::map& start_pos, const std::map& goal_pos, trajectory_msgs::msg::JointTrajectory& joint_trajectory, - const std::string& group_name, const double& velocity_scaling_factor, - const double& acceleration_scaling_factor, const double& sampling_time) + const double& velocity_scaling_factor, const double& acceleration_scaling_factor, + const double& sampling_time) { // initialize joint names for (const auto& item : goal_pos) @@ -149,10 +143,9 @@ void TrajectoryGeneratorPTP::planPTP(const std::map& start_ { // create vecocity profile if necessary velocity_profile.insert(std::make_pair( - joint_name, - VelocityProfileATrap(velocity_scaling_factor * most_strict_limits_.at(group_name).max_velocity, - acceleration_scaling_factor * most_strict_limits_.at(group_name).max_acceleration, - acceleration_scaling_factor * most_strict_limits_.at(group_name).max_deceleration))); + joint_name, VelocityProfileATrap(velocity_scaling_factor * most_strict_limit_.max_velocity, + acceleration_scaling_factor * most_strict_limit_.max_acceleration, + acceleration_scaling_factor * most_strict_limit_.max_deceleration))); velocity_profile.at(joint_name).SetProfile(start_pos.at(joint_name), goal_pos.at(joint_name)); if (velocity_profile.at(joint_name).Duration() > max_duration) @@ -272,7 +265,7 @@ void TrajectoryGeneratorPTP::plan(const planning_scene::PlanningSceneConstPtr& / const double& sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) { // plan the ptp trajectory - planPTP(plan_info.start_joint_position, plan_info.goal_joint_position, joint_trajectory, plan_info.group_name, + planPTP(plan_info.start_joint_position, plan_info.goal_joint_position, joint_trajectory, req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, sampling_time); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h index beda550ddc..346a123f77 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h @@ -92,7 +92,7 @@ inline std::string getJointName(size_t joint_number, const std::string& joint_pr */ pilz_industrial_motion_planner::JointLimitsContainer createFakeLimits(const std::vector& joint_names); -inline std::string demangel(char const* name) +inline std::string demangle(char const* name) { return boost::core::demangle(name); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp index 4094df3c8b..c4ebde9407 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp @@ -202,6 +202,7 @@ TEST_F(CommandPlannerTest, CheckPlanningContextRequest) moveit_msgs::msg::MotionPlanRequest req; moveit_msgs::msg::MoveItErrorCodes error_code; + req.group_name = "manipulator"; // Check for the algorithms std::vector algs; planner_instance_->getPlanningAlgorithms(algs); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp index ff8799469b..2987dfabbd 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp @@ -118,7 +118,7 @@ class PlanningContextTest : public ::testing::Test limits.setCartesianLimits(cartesian_limit); planning_context_ = std::unique_ptr( - new typename T::Type_("TestPlanningContext", "TestGroup", robot_model_, limits)); + new typename T::Type_("TestPlanningContext", planning_group_, robot_model_, limits)); // Define and set the current scene planning_scene::PlanningScenePtr scene(new planning_scene::PlanningScene(robot_model_)); @@ -201,9 +201,9 @@ TYPED_TEST(PlanningContextTest, NoRequest) planning_interface::MotionPlanResponse res; bool result = this->planning_context_->solve(res); - EXPECT_FALSE(result) << testutils::demangel(typeid(TypeParam).name()); + EXPECT_FALSE(result) << testutils::demangle(typeid(TypeParam).name()); EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN, res.error_code_.val) - << testutils::demangel(typeid(TypeParam).name()); + << testutils::demangle(typeid(TypeParam).name()); } /** @@ -212,23 +212,23 @@ TYPED_TEST(PlanningContextTest, NoRequest) TYPED_TEST(PlanningContextTest, SolveValidRequest) { planning_interface::MotionPlanResponse res; - planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangel(typeid(TypeParam).name())); + planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangle(typeid(TypeParam).name())); this->planning_context_->setMotionPlanRequest(req); // TODO Formulate valid request bool result = this->planning_context_->solve(res); - EXPECT_TRUE(result) << testutils::demangel(typeid(TypeParam).name()); + EXPECT_TRUE(result) << testutils::demangle(typeid(TypeParam).name()); EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, res.error_code_.val) - << testutils::demangel(typeid(TypeParam).name()); + << testutils::demangle(typeid(TypeParam).name()); planning_interface::MotionPlanDetailedResponse res_detailed; bool result_detailed = this->planning_context_->solve(res_detailed); - EXPECT_TRUE(result_detailed) << testutils::demangel(typeid(TypeParam).name()); + EXPECT_TRUE(result_detailed) << testutils::demangle(typeid(TypeParam).name()); EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, res.error_code_.val) - << testutils::demangel(typeid(TypeParam).name()); + << testutils::demangle(typeid(TypeParam).name()); } /** @@ -237,14 +237,14 @@ TYPED_TEST(PlanningContextTest, SolveValidRequest) TYPED_TEST(PlanningContextTest, SolveValidRequestDetailedResponse) { planning_interface::MotionPlanDetailedResponse res; //<-- Detailed! - planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangel(typeid(TypeParam).name())); + planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangle(typeid(TypeParam).name())); this->planning_context_->setMotionPlanRequest(req); bool result = this->planning_context_->solve(res); - EXPECT_TRUE(result) << testutils::demangel(typeid(TypeParam).name()); + EXPECT_TRUE(result) << testutils::demangle(typeid(TypeParam).name()); EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, res.error_code_.val) - << testutils::demangel(typeid(TypeParam).name()); + << testutils::demangle(typeid(TypeParam).name()); } /** @@ -253,18 +253,18 @@ TYPED_TEST(PlanningContextTest, SolveValidRequestDetailedResponse) TYPED_TEST(PlanningContextTest, SolveOnTerminated) { planning_interface::MotionPlanResponse res; - planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangel(typeid(TypeParam).name())); + planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangle(typeid(TypeParam).name())); this->planning_context_->setMotionPlanRequest(req); bool result_termination = this->planning_context_->terminate(); - EXPECT_TRUE(result_termination) << testutils::demangel(typeid(TypeParam).name()); + EXPECT_TRUE(result_termination) << testutils::demangle(typeid(TypeParam).name()); bool result = this->planning_context_->solve(res); - EXPECT_FALSE(result) << testutils::demangel(typeid(TypeParam).name()); + EXPECT_FALSE(result) << testutils::demangle(typeid(TypeParam).name()); EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED, res.error_code_.val) - << testutils::demangel(typeid(TypeParam).name()); + << testutils::demangle(typeid(TypeParam).name()); } /** @@ -272,7 +272,7 @@ TYPED_TEST(PlanningContextTest, SolveOnTerminated) */ TYPED_TEST(PlanningContextTest, Clear) { - EXPECT_NO_THROW(this->planning_context_->clear()) << testutils::demangel(typeid(TypeParam).name()); + EXPECT_NO_THROW(this->planning_context_->clear()) << testutils::demangle(typeid(TypeParam).name()); } int main(int argc, char** argv) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp index 39d25365d3..8386128e3b 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp @@ -139,9 +139,10 @@ TEST_P(PlanningContextLoadersTest, GetAlgorithm) TEST_P(PlanningContextLoadersTest, LoadContext) { planning_interface::PlanningContextPtr planning_context; + const std::string& group_name = "manipulator"; // Without limits should return false - bool res = planning_context_loader_->loadContext(planning_context, "test", "test"); + bool res = planning_context_loader_->loadContext(planning_context, "test", group_name); EXPECT_EQ(false, res) << "Context returned even when no limits where set"; // After setting the limits this should work @@ -161,7 +162,7 @@ TEST_P(PlanningContextLoadersTest, LoadContext) try { - res = planning_context_loader_->loadContext(planning_context, "test", "test"); + res = planning_context_loader_->loadContext(planning_context, "test", group_name); } catch (std::exception& ex) { diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp index 37f48ba94f..7cc9d49f7b 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp @@ -125,7 +125,7 @@ class TrajectoryBlenderTransitionWindowTest : public testing::Test planner_limits_.setCartesianLimits(cart_limits); // initialize trajectory generators and blender - lin_generator_ = std::make_unique(robot_model_, planner_limits_); + lin_generator_ = std::make_unique(robot_model_, planner_limits_, planning_group_); ASSERT_NE(nullptr, lin_generator_) << "failed to create LIN trajectory generator"; blender_ = std::make_unique(planner_limits_); ASSERT_NE(nullptr, blender_) << "failed to create trajectory blender"; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp index b8260c97d3..0da53b6b00 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp @@ -126,7 +126,7 @@ class TrajectoryGeneratorCIRCTest : public testing::Test planner_limits_.setCartesianLimits(cart_limits); // initialize the LIN trajectory generator - circ_ = std::make_unique(robot_model_, planner_limits_); + circ_ = std::make_unique(robot_model_, planner_limits_, planning_group_); ASSERT_NE(nullptr, circ_) << "failed to create CIRC trajectory generator"; } @@ -282,7 +282,8 @@ TEST_F(TrajectoryGeneratorCIRCTest, TestExceptionErrorCodeMapping) TEST_F(TrajectoryGeneratorCIRCTest, noLimits) { LimitsContainer planner_limits; - EXPECT_THROW(TrajectoryGeneratorCIRC(this->robot_model_, planner_limits), TrajectoryGeneratorInvalidLimitsException); + EXPECT_THROW(TrajectoryGeneratorCIRC(this->robot_model_, planner_limits, planning_group_), + TrajectoryGeneratorInvalidLimitsException); } /** diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp index 01f2eccb3f..06eabc333a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp @@ -131,7 +131,8 @@ class TrajectoryGeneratorCommonTest : public ::testing::Test planner_limits.setCartesianLimits(cart_limits); // create planner instance - trajectory_generator_ = std::unique_ptr(new typename T::Type_(robot_model_, planner_limits)); + trajectory_generator_ = + std::unique_ptr(new typename T::Type_(robot_model_, planner_limits, planning_group_)); ASSERT_NE(nullptr, trajectory_generator_) << "failed to create trajectory generator"; // create a valid motion plan request with goal in joint space as basis for diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp index 5b92b83faf..583466e43f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp @@ -127,7 +127,7 @@ class TrajectoryGeneratorLINTest : public testing::Test planner_limits_.setCartesianLimits(cart_limits); // initialize the LIN trajectory generator - lin_ = std::make_unique(robot_model_, planner_limits_); + lin_ = std::make_unique(robot_model_, planner_limits_, planning_group_); ASSERT_NE(nullptr, lin_) << "Failed to create LIN trajectory generator."; } @@ -394,7 +394,7 @@ TEST_F(TrajectoryGeneratorLINTest, CtorNoLimits) { pilz_industrial_motion_planner::LimitsContainer planner_limits; - EXPECT_THROW(pilz_industrial_motion_planner::TrajectoryGeneratorLIN(robot_model_, planner_limits), + EXPECT_THROW(pilz_industrial_motion_planner::TrajectoryGeneratorLIN(robot_model_, planner_limits, planning_group_), pilz_industrial_motion_planner::TrajectoryGeneratorInvalidLimitsException); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp index 3478abddce..e2805d0fda 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp @@ -114,7 +114,7 @@ class TrajectoryGeneratorPTPTest : public testing::Test // create the trajectory generator planner_limits_.setJointLimits(joint_limits); - ptp_ = std::make_unique(robot_model_, planner_limits_); + ptp_ = std::make_unique(robot_model_, planner_limits_, planning_group_); ASSERT_NE(nullptr, ptp_); } @@ -174,7 +174,8 @@ TEST_F(TrajectoryGeneratorPTPTest, TestExceptionErrorCodeMapping) TEST_F(TrajectoryGeneratorPTPTest, noLimits) { LimitsContainer planner_limits; - EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits), TrajectoryGeneratorInvalidLimitsException); + EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits, planning_group_), + TrajectoryGeneratorInvalidLimitsException); } /** @@ -226,7 +227,8 @@ TEST_F(TrajectoryGeneratorPTPTest, missingVelocityLimits) } planner_limits.setJointLimits(joint_limits); - EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits), TrajectoryGeneratorInvalidLimitsException); + EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits, planning_group_), + TrajectoryGeneratorInvalidLimitsException); } /** @@ -249,7 +251,8 @@ TEST_F(TrajectoryGeneratorPTPTest, missingDecelerationimits) } planner_limits.setJointLimits(joint_limits); - EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits), TrajectoryGeneratorInvalidLimitsException); + EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits, planning_group_), + TrajectoryGeneratorInvalidLimitsException); } /** @@ -292,7 +295,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testInsufficientLimit) EXPECT_THROW( { std::unique_ptr ptp_error( - new TrajectoryGeneratorPTP(robot_model_, insufficient_planner_limits)); + new TrajectoryGeneratorPTP(robot_model_, insufficient_planner_limits, planning_group_)); }, TrajectoryGeneratorInvalidLimitsException); @@ -331,7 +334,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testInsufficientLimit) EXPECT_NO_THROW({ std::unique_ptr ptp_no_error( - new TrajectoryGeneratorPTP(robot_model_, sufficient_planner_limits)); + new TrajectoryGeneratorPTP(robot_model_, sufficient_planner_limits, planning_group_)); }); } @@ -522,7 +525,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testScalingFactor) planner_limits.setJointLimits(joint_limits); // create the generator with new limits - ptp_ = std::make_unique(robot_model_, planner_limits); + ptp_ = std::make_unique(robot_model_, planner_limits, planning_group_); planning_interface::MotionPlanResponse res; planning_interface::MotionPlanRequest req; diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index 7c70bd0757..16027a3aed 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -117,6 +117,7 @@ class MotionPlanningFrame : public QWidget static const int ITEM_TYPE_SCENE = 1; static const int ITEM_TYPE_QUERY = 2; + void initFromMoveGroupNS(); void constructPlanningRequest(moveit_msgs::msg::MotionPlanRequest& mreq); void updateSceneMarkers(float wall_dt, float ros_dt); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index 9db5833343..deb228ec14 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -206,14 +206,6 @@ void MotionPlanningDisplay::onInitialize() { PlanningSceneDisplay::onInitialize(); - // Prepare database parameters - if (!node_->has_parameter("warehouse_host")) - node_->declare_parameter("warehouse_host", "127.0.0.1"); - if (!node_->has_parameter("warehouse_plugin")) - node_->declare_parameter("warehouse_plugin", "warehouse_ros_mongo::MongoDatabaseConnection"); - if (!node_->has_parameter("warehouse_port")) - node_->declare_parameter("warehouse_port", 33829); - // Planned Path Display trajectory_visual_->onInitialize(node_, planning_scene_node_, context_); QColor qcolor = attached_body_color_property_->getColor(); @@ -246,18 +238,6 @@ void MotionPlanningDisplay::onInitialize() rviz_common::WindowManagerInterface* window_context = context_->getWindowManager(); frame_ = new MotionPlanningFrame(this, context_, window_context ? window_context->getParentWindow() : nullptr); - std::string host_param; - if (node_->get_parameter("warehouse_host", host_param)) - { - frame_->ui_->database_host->setText(QString::fromStdString(host_param)); - } - - int port; - if (node_->get_parameter("warehouse_port", port)) - { - frame_->ui_->database_port->setValue(port); - } - connect(frame_, SIGNAL(configChanged()), this->getModel(), SIGNAL(configChanged())); resetStatusTextColor(); addStatusText("Initialized."); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 50b668a5ba..54058ad8eb 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -71,7 +71,14 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz_c return; } node_ = ros_node_abstraction->get_raw_node(); - clear_octomap_service_client_ = node_->create_client(move_group::CLEAR_OCTOMAP_SERVICE_NAME); + + // Prepare database parameters + if (!node_->has_parameter("warehouse_host")) + node_->declare_parameter("warehouse_host", "127.0.0.1"); + if (!node_->has_parameter("warehouse_plugin")) + node_->declare_parameter("warehouse_plugin", "warehouse_ros_mongo::MongoDatabaseConnection"); + if (!node_->has_parameter("warehouse_port")) + node_->declare_parameter("warehouse_port", 33829); // set up the GUI ui_->setupUi(this); @@ -89,17 +96,6 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz_c connect(planning_display_, SIGNAL(queryStartStateChanged()), joints_tab_, SLOT(queryStartStateChanged())); connect(planning_display_, SIGNAL(queryGoalStateChanged()), joints_tab_, SLOT(queryGoalStateChanged())); - // Set initial velocity and acceleration scaling factors from ROS parameters - double factor; - node_->get_parameter_or("robot_description_planning.default_velocity_scaling_factor", factor, 0.1); - ui_->velocity_scaling_factor->setValue(factor); - node_->get_parameter_or("robot_description_planning.default_acceleration_scaling_factor", factor, 0.1); - ui_->acceleration_scaling_factor->setValue(factor); - - // Query default planning pipeline id - node_->get_parameter(planning_display_->getMoveGroupNS() + "/move_group/default_planning_pipeline", - default_planning_pipeline_); - // connect buttons to actions; each action usually registers the function pointer for the actual computation, // to keep the GUI more responsive (using the background job processing) connect(ui_->plan_button, SIGNAL(clicked()), this, SLOT(planButtonClicked())); @@ -204,16 +200,11 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz_c known_collision_objects_version_ = 0; - planning_scene_publisher_ = node_->create_publisher("planning_scene", 1); - planning_scene_world_publisher_ = - node_->create_publisher("planning_scene_world", 1); + initFromMoveGroupNS(); object_recognition_client_ = rclcpp_action::create_client( node_, OBJECT_RECOGNITION_ACTION); - object_recognition_subscriber_ = node_->create_subscription( - "recognized_object_array", 1, std::bind(&MotionPlanningFrame::listenDetectedObjects, this, std::placeholders::_1)); - if (object_recognition_client_) { if (!object_recognition_client_->wait_for_action_server(std::chrono::seconds(3))) @@ -587,11 +578,58 @@ void MotionPlanningFrame::enable() ui_->library_label->setStyleSheet("QLabel { color : red; font: bold }"); ui_->object_status->setText(""); + const std::string new_ns = planning_display_->getMoveGroupNS(); + if (node_->get_namespace() != new_ns) + { + RCLCPP_INFO(LOGGER, "MoveGroup namespace changed: %s -> %s. Reloading params.", node_->get_namespace(), + new_ns.c_str()); + initFromMoveGroupNS(); + } + // activate the frame if (parentWidget()) parentWidget()->show(); } +// (re)initialize after MotionPlanningDisplay::changedMoveGroupNS() +// Should be called from constructor and enable() only +void MotionPlanningFrame::initFromMoveGroupNS() +{ + // Create namespace-dependent services, topics, and subscribers + clear_octomap_service_client_ = node_->create_client(move_group::CLEAR_OCTOMAP_SERVICE_NAME); + + object_recognition_subscriber_ = node_->create_subscription( + "recognized_object_array", 1, std::bind(&MotionPlanningFrame::listenDetectedObjects, this, std::placeholders::_1)); + + planning_scene_publisher_ = node_->create_publisher("planning_scene", 1); + planning_scene_world_publisher_ = + node_->create_publisher("planning_scene_world", 1); + + // Set initial velocity and acceleration scaling factors from ROS parameters + double factor; + node_->get_parameter_or("robot_description_planning.default_velocity_scaling_factor", factor, 0.1); + ui_->velocity_scaling_factor->setValue(factor); + node_->get_parameter_or("robot_description_planning.default_acceleration_scaling_factor", factor, 0.1); + ui_->acceleration_scaling_factor->setValue(factor); + + // Fetch parameters from private move_group sub space + std::string host_param; + if (node_->get_parameter("warehouse_host", host_param)) + { + ui_->database_host->setText(QString::fromStdString(host_param)); + } + + int port; + if (node_->get_parameter("warehouse_port", port)) + { + ui_->database_port->setValue(port); + } + + // Query default planning pipeline id + node_->get_parameter(planning_display_->getMoveGroupNS() + "/move_group/default_planning_pipeline", + default_planning_pipeline_); +} + void MotionPlanningFrame::disable() { move_group_.reset(); diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml index 2a269a075c..1efc010d48 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml @@ -17,8 +17,8 @@ - - - + +