From 2344d99d5660fefb0efca3d65046b424b4b0322a Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Tue, 5 Mar 2019 16:38:45 -0700 Subject: [PATCH 1/8] Document release in MIGRATION.md Make it clear that future migration notes go in a new section --- MIGRATION.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/MIGRATION.md b/MIGRATION.md index 91822de55f..25f6d842de 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -2,6 +2,8 @@ API changes in MoveIt! releases +## ROS Noetic (upcoming changes in master) + ## ROS Melodic - Migration to ``tf2`` API. From a662538637d39f318fb0f71a167686ac82014e9d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 8 Mar 2019 19:32:40 +0100 Subject: [PATCH 2/8] Fix alignment of Eigen transforms (#1382) * Fix alignment of Eigen transforms With AVX support, Eigen requires a 32-byte alignment, instead of the default 16-byte alignment. * use constexpr --- moveit_core/robot_state/src/robot_state.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 7837760f20..acbdf3a5ce 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -87,17 +87,25 @@ RobotState::~RobotState() void RobotState::allocMemory() { + static_assert((sizeof(Eigen::Isometry3d) / EIGEN_MAX_ALIGN_BYTES) * EIGEN_MAX_ALIGN_BYTES == + sizeof(Eigen::Isometry3d), + "sizeof(Eigen::Isometry3d) should be a multiple of EIGEN_MAX_ALIGN_BYTES"); + + constexpr unsigned int extra_alignment_bytes = EIGEN_MAX_ALIGN_BYTES - 1; // memory for the dirty joint transforms const int nr_doubles_for_dirty_joint_transforms = 1 + robot_model_->getJointModelCount() / (sizeof(double) / sizeof(unsigned char)); const size_t bytes = sizeof(Eigen::Isometry3d) * (robot_model_->getJointModelCount() + robot_model_->getLinkModelCount() + robot_model_->getLinkGeometryCount()) + - sizeof(double) * (robot_model_->getVariableCount() * 3 + nr_doubles_for_dirty_joint_transforms) + 15; + sizeof(double) * (robot_model_->getVariableCount() * 3 + nr_doubles_for_dirty_joint_transforms) + + extra_alignment_bytes; memory_ = malloc(bytes); - // make the memory for transforms align at 16 bytes - variable_joint_transforms_ = reinterpret_cast(((uintptr_t)memory_ + 15) & ~(uintptr_t)0x0F); + // make the memory for transforms align at EIGEN_MAX_ALIGN_BYTES + // https://eigen.tuxfamily.org/dox/classEigen_1_1aligned__allocator.html + variable_joint_transforms_ = reinterpret_cast(((uintptr_t)memory_ + extra_alignment_bytes) & + ~(uintptr_t)extra_alignment_bytes); global_link_transforms_ = variable_joint_transforms_ + robot_model_->getJointModelCount(); global_collision_body_transforms_ = global_link_transforms_ + robot_model_->getLinkModelCount(); dirty_joint_transforms_ = From a20b3632ef74ff57028a16ff066cb007ab1c1a42 Mon Sep 17 00:00:00 2001 From: "Isaac I.Y. Saito" <130s@users.noreply.github.com> Date: Fri, 8 Mar 2019 13:29:12 -0800 Subject: [PATCH 3/8] [master] Update version info to 1.0.1 (#1379) * changelog 1.0.1 * tag 1.0.1 --- moveit/CHANGELOG.rst | 10 ++++++++++ moveit/package.xml | 2 +- moveit_commander/CHANGELOG.rst | 5 +++++ moveit_commander/package.xml | 2 +- moveit_core/CHANGELOG.rst | 6 ++++++ moveit_core/package.xml | 2 +- moveit_experimental/CHANGELOG.rst | 3 +++ moveit_experimental/package.xml | 2 +- moveit_kinematics/CHANGELOG.rst | 5 +++++ moveit_kinematics/package.xml | 2 +- moveit_planners/chomp/chomp_interface/CHANGELOG.rst | 5 +++++ moveit_planners/chomp/chomp_interface/package.xml | 2 +- .../chomp/chomp_motion_planner/CHANGELOG.rst | 5 +++++ moveit_planners/chomp/chomp_motion_planner/package.xml | 2 +- .../chomp/chomp_optimizer_adapter/CHANGELOG.rst | 6 ++++++ .../chomp/chomp_optimizer_adapter/package.xml | 2 +- moveit_planners/moveit_planners/CHANGELOG.rst | 3 +++ moveit_planners/moveit_planners/package.xml | 2 +- moveit_planners/ompl/CHANGELOG.rst | 5 +++++ moveit_planners/ompl/package.xml | 2 +- .../moveit_controller_manager_example/CHANGELOG.rst | 5 +++++ .../moveit_controller_manager_example/package.xml | 2 +- .../moveit_fake_controller_manager/CHANGELOG.rst | 5 +++++ .../moveit_fake_controller_manager/package.xml | 2 +- moveit_plugins/moveit_plugins/CHANGELOG.rst | 3 +++ moveit_plugins/moveit_plugins/package.xml | 2 +- .../moveit_ros_control_interface/CHANGELOG.rst | 3 +++ .../moveit_ros_control_interface/package.xml | 2 +- .../moveit_simple_controller_manager/CHANGELOG.rst | 5 +++++ .../moveit_simple_controller_manager/package.xml | 2 +- moveit_ros/benchmarks/CHANGELOG.rst | 5 +++++ moveit_ros/benchmarks/package.xml | 2 +- moveit_ros/manipulation/CHANGELOG.rst | 5 +++++ moveit_ros/manipulation/package.xml | 2 +- moveit_ros/move_group/CHANGELOG.rst | 5 +++++ moveit_ros/move_group/package.xml | 2 +- moveit_ros/moveit_ros/CHANGELOG.rst | 3 +++ moveit_ros/moveit_ros/package.xml | 2 +- moveit_ros/perception/CHANGELOG.rst | 5 +++++ moveit_ros/perception/package.xml | 2 +- moveit_ros/planning/CHANGELOG.rst | 5 +++++ moveit_ros/planning/package.xml | 2 +- moveit_ros/planning_interface/CHANGELOG.rst | 5 +++++ moveit_ros/planning_interface/package.xml | 2 +- moveit_ros/robot_interaction/CHANGELOG.rst | 5 +++++ moveit_ros/robot_interaction/package.xml | 2 +- moveit_ros/visualization/CHANGELOG.rst | 5 +++++ moveit_ros/visualization/package.xml | 2 +- moveit_ros/warehouse/CHANGELOG.rst | 5 +++++ moveit_ros/warehouse/package.xml | 2 +- moveit_runtime/CHANGELOG.rst | 3 +++ moveit_runtime/package.xml | 2 +- moveit_setup_assistant/CHANGELOG.rst | 6 ++++++ moveit_setup_assistant/package.xml | 2 +- 54 files changed, 158 insertions(+), 27 deletions(-) diff --git a/moveit/CHANGELOG.rst b/moveit/CHANGELOG.rst index e9bccfd53a..9f03a155e1 100644 --- a/moveit/CHANGELOG.rst +++ b/moveit/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package moveit ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ +* [fix] segfault in chomp adapter (`#1377 `_) +* [capability] Graphically print current robot joint states with joint limits (`#1358 `_) +* [capability] python PlanningSceneInterface.add_cylinder() (`#1372 `_) +* [capability] Add time-optimal trajectory parameterization https://github.com/ros-planning/moveit/pull/1365 +* [capability] FCL as a plugin https://github.com/ros-planning/moveit/pull/1370 +* [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) +* Contributors: Dave Coleman, Robert Haschke, Yu, Yan + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit/package.xml b/moveit/package.xml index ffc36a5004..0a60b4860e 100644 --- a/moveit/package.xml +++ b/moveit/package.xml @@ -1,7 +1,7 @@ moveit - 1.0.0 + 1.0.1 Meta package that contains all essential package of MoveIt!. Until Summer 2016 MoveIt! had been developed over multiple repositories, where developers' usability and maintenance effort was non-trivial. See the detailed discussion for the merge of several repositories. Dave Coleman Michael Ferguson diff --git a/moveit_commander/CHANGELOG.rst b/moveit_commander/CHANGELOG.rst index 30ad770834..4184e8de90 100644 --- a/moveit_commander/CHANGELOG.rst +++ b/moveit_commander/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_commander ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ +* [capability] python PlanningSceneInterface.add_cylinder() (`#1372 `_) +* Contributors: Robert Haschke + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_commander/package.xml b/moveit_commander/package.xml index 8d5f1ebd3c..6680232adc 100644 --- a/moveit_commander/package.xml +++ b/moveit_commander/package.xml @@ -1,6 +1,6 @@ moveit_commander - 1.0.0 + 1.0.1 Python interfaces to MoveIt Ioan Sucan diff --git a/moveit_core/CHANGELOG.rst b/moveit_core/CHANGELOG.rst index 32608da0a5..b9990334d2 100644 --- a/moveit_core/CHANGELOG.rst +++ b/moveit_core/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ +* [capability] Graphically print current robot joint states with joint limits (`#1358 `_) +* [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) +* Contributors: Dave Coleman, Robert Haschke, Yu, Yan + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_core/package.xml b/moveit_core/package.xml index 97da46e163..593b495ed6 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -1,6 +1,6 @@ moveit_core - 1.0.0 + 1.0.1 Core libraries used by MoveIt! Ioan Sucan Sachin Chitta diff --git a/moveit_experimental/CHANGELOG.rst b/moveit_experimental/CHANGELOG.rst index d2c1bd2382..813d8a2015 100644 --- a/moveit_experimental/CHANGELOG.rst +++ b/moveit_experimental/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_experimental ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_experimental/package.xml b/moveit_experimental/package.xml index 8e4589f447..2ba94edb00 100644 --- a/moveit_experimental/package.xml +++ b/moveit_experimental/package.xml @@ -1,6 +1,6 @@ moveit_experimental - 1.0.0 + 1.0.1 Experimental packages for MoveIt! MoveIt! Release Team diff --git a/moveit_kinematics/CHANGELOG.rst b/moveit_kinematics/CHANGELOG.rst index 8efefbbb28..b827103069 100644 --- a/moveit_kinematics/CHANGELOG.rst +++ b/moveit_kinematics/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_kinematics ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ +* [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) +* Contributors: Robert Haschke, Yu, Yan + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml index 659f0f8bff..9b7e7205fb 100644 --- a/moveit_kinematics/package.xml +++ b/moveit_kinematics/package.xml @@ -1,7 +1,7 @@ moveit_kinematics - 1.0.0 + 1.0.1 Package for all inverse kinematics solvers in MoveIt! Dave Coleman diff --git a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst index 3e7004be84..3b2e6e9912 100644 --- a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package chomp_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ +* [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) +* Contributors: Robert Haschke, Yu, Yan + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_planners/chomp/chomp_interface/package.xml b/moveit_planners/chomp/chomp_interface/package.xml index 296f4aee60..e7f232010e 100644 --- a/moveit_planners/chomp/chomp_interface/package.xml +++ b/moveit_planners/chomp/chomp_interface/package.xml @@ -2,7 +2,7 @@ moveit_planners_chomp The interface for using CHOMP within MoveIt! - 1.0.0 + 1.0.1 Gil Jones Chittaranjan Srinivas Swaminathan diff --git a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst index f360882bf7..0dc05a2151 100644 --- a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package chomp_motion_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ +* [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) +* Contributors: Robert Haschke, Yu, Yan + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_planners/chomp/chomp_motion_planner/package.xml b/moveit_planners/chomp/chomp_motion_planner/package.xml index 18d62ea710..8abb126185 100644 --- a/moveit_planners/chomp/chomp_motion_planner/package.xml +++ b/moveit_planners/chomp/chomp_motion_planner/package.xml @@ -2,7 +2,7 @@ chomp_motion_planner chomp_motion_planner - 1.0.0 + 1.0.1 Gil Jones Mrinal Kalakrishnan diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst index 444e9b6952..5ac9064e2d 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_chomp_optimizer_adapter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ +* [fix] segfault in chomp adapter (`#1377 `_) +* [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) +* Contributors: Robert Haschke, Yu, Yan + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml index 2a4145dbdd..f5a6d016a7 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml +++ b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml @@ -3,7 +3,7 @@ moveit_chomp_optimizer_adapter MoveIt planning request adapter utilizing chomp for solution optimization - 1.0.0 + 1.0.1 Raghavender Sahdev Raghavender Sahdev MoveIt! Release Team diff --git a/moveit_planners/moveit_planners/CHANGELOG.rst b/moveit_planners/moveit_planners/CHANGELOG.rst index 38478b5257..68168623d7 100644 --- a/moveit_planners/moveit_planners/CHANGELOG.rst +++ b/moveit_planners/moveit_planners/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_planners ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_planners/moveit_planners/package.xml b/moveit_planners/moveit_planners/package.xml index 8ccf7faf32..77d65918bf 100644 --- a/moveit_planners/moveit_planners/package.xml +++ b/moveit_planners/moveit_planners/package.xml @@ -1,6 +1,6 @@ moveit_planners - 1.0.0 + 1.0.1 Metapacakge that installs all available planners for MoveIt Ioan Sucan Sachin Chitta diff --git a/moveit_planners/ompl/CHANGELOG.rst b/moveit_planners/ompl/CHANGELOG.rst index fa08d5f375..06a4259f1c 100644 --- a/moveit_planners/ompl/CHANGELOG.rst +++ b/moveit_planners/ompl/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_planners_ompl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ +* [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) +* Contributors: Robert Haschke, Yu, Yan + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_planners/ompl/package.xml b/moveit_planners/ompl/package.xml index 1ad8bf24f8..1f2f7e6fc0 100644 --- a/moveit_planners/ompl/package.xml +++ b/moveit_planners/ompl/package.xml @@ -1,6 +1,6 @@ moveit_planners_ompl - 1.0.0 + 1.0.1 MoveIt! interface to OMPL Ioan Sucan diff --git a/moveit_plugins/moveit_controller_manager_example/CHANGELOG.rst b/moveit_plugins/moveit_controller_manager_example/CHANGELOG.rst index 963a9f2548..c85fcf85ac 100644 --- a/moveit_plugins/moveit_controller_manager_example/CHANGELOG.rst +++ b/moveit_plugins/moveit_controller_manager_example/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_controller_manager_example ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ +* [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) +* Contributors: Yu, Yan + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_plugins/moveit_controller_manager_example/package.xml b/moveit_plugins/moveit_controller_manager_example/package.xml index 8f6e725a97..a7988ab6d6 100644 --- a/moveit_plugins/moveit_controller_manager_example/package.xml +++ b/moveit_plugins/moveit_controller_manager_example/package.xml @@ -1,6 +1,6 @@ moveit_controller_manager_example - 1.0.0 + 1.0.1 An example controller manager plugin for MoveIt. This is not functional code. Ioan Sucan diff --git a/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst index 9bcf9995ee..887619d6ee 100644 --- a/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst +++ b/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_fake_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ +* [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) +* Contributors: Robert Haschke, Yu, Yan + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_plugins/moveit_fake_controller_manager/package.xml b/moveit_plugins/moveit_fake_controller_manager/package.xml index 88d80ef12e..05c2662a48 100644 --- a/moveit_plugins/moveit_fake_controller_manager/package.xml +++ b/moveit_plugins/moveit_fake_controller_manager/package.xml @@ -1,6 +1,6 @@ moveit_fake_controller_manager - 1.0.0 + 1.0.1 A fake controller manager plugin for MoveIt. Ioan Sucan diff --git a/moveit_plugins/moveit_plugins/CHANGELOG.rst b/moveit_plugins/moveit_plugins/CHANGELOG.rst index d078d7dfec..4905296463 100644 --- a/moveit_plugins/moveit_plugins/CHANGELOG.rst +++ b/moveit_plugins/moveit_plugins/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_plugins/moveit_plugins/package.xml b/moveit_plugins/moveit_plugins/package.xml index b24c6be196..265fa36725 100644 --- a/moveit_plugins/moveit_plugins/package.xml +++ b/moveit_plugins/moveit_plugins/package.xml @@ -1,6 +1,6 @@ moveit_plugins - 1.0.0 + 1.0.1 Metapackage for MoveIt! plugins. Michael Ferguson diff --git a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst index e57a0859ee..d6312849f9 100644 --- a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst +++ b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_ros_control_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_plugins/moveit_ros_control_interface/package.xml b/moveit_plugins/moveit_ros_control_interface/package.xml index 7c3aeba416..98d0206d4b 100644 --- a/moveit_plugins/moveit_ros_control_interface/package.xml +++ b/moveit_plugins/moveit_ros_control_interface/package.xml @@ -1,7 +1,7 @@ moveit_ros_control_interface - 1.0.0 + 1.0.1 ros_control controller manager interface for MoveIt! Mathias Lüdtke diff --git a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst index aacc420b35..1f699681e5 100644 --- a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst +++ b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_simple_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ +* [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) +* Contributors: Yu, Yan + 1.0.0 (2019-02-24) ------------------ * [maintenance] cleanup SimpleControllerManager https://github.com/ros-planning/moveit/pull/1352 diff --git a/moveit_plugins/moveit_simple_controller_manager/package.xml b/moveit_plugins/moveit_simple_controller_manager/package.xml index ff1e5706a2..aad84299c3 100644 --- a/moveit_plugins/moveit_simple_controller_manager/package.xml +++ b/moveit_plugins/moveit_simple_controller_manager/package.xml @@ -1,6 +1,6 @@ moveit_simple_controller_manager - 1.0.0 + 1.0.1 A generic, simple controller manager plugin for MoveIt. Michael Ferguson diff --git a/moveit_ros/benchmarks/CHANGELOG.rst b/moveit_ros/benchmarks/CHANGELOG.rst index fedf54fc1a..98e4914f29 100644 --- a/moveit_ros/benchmarks/CHANGELOG.rst +++ b/moveit_ros/benchmarks/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_ros_benchmarks ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ +* [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) +* Contributors: Robert Haschke, Yu, Yan + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_ros/benchmarks/package.xml b/moveit_ros/benchmarks/package.xml index 7c49c242c0..cbb64dbe81 100644 --- a/moveit_ros/benchmarks/package.xml +++ b/moveit_ros/benchmarks/package.xml @@ -1,6 +1,6 @@ moveit_ros_benchmarks - 1.0.0 + 1.0.1 Enhanced tools for benchmarks in MoveIt! diff --git a/moveit_ros/manipulation/CHANGELOG.rst b/moveit_ros/manipulation/CHANGELOG.rst index 4630b6a76e..3a939ead3b 100644 --- a/moveit_ros/manipulation/CHANGELOG.rst +++ b/moveit_ros/manipulation/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_ros_manipulation ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ +* [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) +* Contributors: Yu, Yan + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_ros/manipulation/package.xml b/moveit_ros/manipulation/package.xml index 4e07ccc8e5..d8168e7329 100644 --- a/moveit_ros/manipulation/package.xml +++ b/moveit_ros/manipulation/package.xml @@ -1,6 +1,6 @@ moveit_ros_manipulation - 1.0.0 + 1.0.1 Components of MoveIt! used for manipulation Ioan Sucan diff --git a/moveit_ros/move_group/CHANGELOG.rst b/moveit_ros/move_group/CHANGELOG.rst index 26534f9a83..b2aa5af725 100644 --- a/moveit_ros/move_group/CHANGELOG.rst +++ b/moveit_ros/move_group/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_ros_move_group ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ +* [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) +* Contributors: Yu, Yan + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_ros/move_group/package.xml b/moveit_ros/move_group/package.xml index 9d440040b7..ffed75a12d 100644 --- a/moveit_ros/move_group/package.xml +++ b/moveit_ros/move_group/package.xml @@ -1,6 +1,6 @@ moveit_ros_move_group - 1.0.0 + 1.0.1 The move_group node for MoveIt Ioan Sucan Sachin Chitta diff --git a/moveit_ros/moveit_ros/CHANGELOG.rst b/moveit_ros/moveit_ros/CHANGELOG.rst index a46b2be50e..d3a8959214 100644 --- a/moveit_ros/moveit_ros/CHANGELOG.rst +++ b/moveit_ros/moveit_ros/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_ros/moveit_ros/package.xml b/moveit_ros/moveit_ros/package.xml index a173297704..a64adef04d 100644 --- a/moveit_ros/moveit_ros/package.xml +++ b/moveit_ros/moveit_ros/package.xml @@ -1,6 +1,6 @@ moveit_ros - 1.0.0 + 1.0.1 Components of MoveIt! that use ROS Ioan Sucan Sachin Chitta diff --git a/moveit_ros/perception/CHANGELOG.rst b/moveit_ros/perception/CHANGELOG.rst index 3b24310ab9..9df15123be 100644 --- a/moveit_ros/perception/CHANGELOG.rst +++ b/moveit_ros/perception/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_ros_perception ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ +* [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) +* Contributors: Yu, Yan + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_ros/perception/package.xml b/moveit_ros/perception/package.xml index e1730b1a6e..e09262f3d6 100644 --- a/moveit_ros/perception/package.xml +++ b/moveit_ros/perception/package.xml @@ -1,6 +1,6 @@ moveit_ros_perception - 1.0.0 + 1.0.1 Components of MoveIt! connecting to perception Ioan Sucan diff --git a/moveit_ros/planning/CHANGELOG.rst b/moveit_ros/planning/CHANGELOG.rst index 61d51e0921..e2bd85fd4d 100644 --- a/moveit_ros/planning/CHANGELOG.rst +++ b/moveit_ros/planning/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_ros_planning ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ +* [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) +* Contributors: Robert Haschke, Yu, Yan + 1.0.0 (2019-02-24) ------------------ * [maintenance] Travis: enable warnings and catkin_lint checker (`#1332 `_) diff --git a/moveit_ros/planning/package.xml b/moveit_ros/planning/package.xml index 56458ed2fa..b6e58d56d0 100644 --- a/moveit_ros/planning/package.xml +++ b/moveit_ros/planning/package.xml @@ -1,7 +1,7 @@ moveit_ros_planning - 1.0.0 + 1.0.1 Planning components of MoveIt! that use ROS Ioan Sucan Sachin Chitta diff --git a/moveit_ros/planning_interface/CHANGELOG.rst b/moveit_ros/planning_interface/CHANGELOG.rst index 1c128aa904..2e0e72f5f1 100644 --- a/moveit_ros/planning_interface/CHANGELOG.rst +++ b/moveit_ros/planning_interface/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_ros_planning_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ +* [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) +* Contributors: Robert Haschke, Yu, Yan + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml index 0c97aa6b6f..da14d37605 100644 --- a/moveit_ros/planning_interface/package.xml +++ b/moveit_ros/planning_interface/package.xml @@ -1,6 +1,6 @@ moveit_ros_planning_interface - 1.0.0 + 1.0.1 Components of MoveIt! that offer simpler interfaces to planning and execution Ioan Sucan diff --git a/moveit_ros/robot_interaction/CHANGELOG.rst b/moveit_ros/robot_interaction/CHANGELOG.rst index 1a38811510..93a596b313 100644 --- a/moveit_ros/robot_interaction/CHANGELOG.rst +++ b/moveit_ros/robot_interaction/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_ros_robot_interaction ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ +* [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) +* Contributors: Yu, Yan + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_ros/robot_interaction/package.xml b/moveit_ros/robot_interaction/package.xml index 04fbbd1fee..25cc5b7107 100644 --- a/moveit_ros/robot_interaction/package.xml +++ b/moveit_ros/robot_interaction/package.xml @@ -1,6 +1,6 @@ moveit_ros_robot_interaction - 1.0.0 + 1.0.1 Components of MoveIt! that offer interaction via interactive markers Ioan Sucan diff --git a/moveit_ros/visualization/CHANGELOG.rst b/moveit_ros/visualization/CHANGELOG.rst index 1b37aa80fe..4e24d61666 100644 --- a/moveit_ros/visualization/CHANGELOG.rst +++ b/moveit_ros/visualization/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_ros_visualization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ +* [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) +* Contributors: Isaac Robert Haschke, Yu, Yan + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_ros/visualization/package.xml b/moveit_ros/visualization/package.xml index 9881604be5..7b93f92cb1 100644 --- a/moveit_ros/visualization/package.xml +++ b/moveit_ros/visualization/package.xml @@ -1,6 +1,6 @@ moveit_ros_visualization - 1.0.0 + 1.0.1 Components of MoveIt! that offer visualization Ioan Sucan diff --git a/moveit_ros/warehouse/CHANGELOG.rst b/moveit_ros/warehouse/CHANGELOG.rst index 78d377fd64..e3d3c6dd37 100644 --- a/moveit_ros/warehouse/CHANGELOG.rst +++ b/moveit_ros/warehouse/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_ros_warehouse ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ +* [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) +* Contributors: Robert Haschke, Yu, Yan + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_ros/warehouse/package.xml b/moveit_ros/warehouse/package.xml index 6a1dae81a2..976cdd1c79 100644 --- a/moveit_ros/warehouse/package.xml +++ b/moveit_ros/warehouse/package.xml @@ -1,6 +1,6 @@ moveit_ros_warehouse - 1.0.0 + 1.0.1 Components of MoveIt! connecting to MongoDB Ioan Sucan diff --git a/moveit_runtime/CHANGELOG.rst b/moveit_runtime/CHANGELOG.rst index cb99f5434f..bb959b1866 100644 --- a/moveit_runtime/CHANGELOG.rst +++ b/moveit_runtime/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_runtime ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_runtime/package.xml b/moveit_runtime/package.xml index a8b37053aa..8efd4a159f 100644 --- a/moveit_runtime/package.xml +++ b/moveit_runtime/package.xml @@ -1,7 +1,7 @@ moveit_runtime - 1.0.0 + 1.0.1 moveit_runtime meta package contains MoveIt! packages that are essential for its runtime (e.g. running MoveIt! on robots). Isaac I. Y. Saito diff --git a/moveit_setup_assistant/CHANGELOG.rst b/moveit_setup_assistant/CHANGELOG.rst index bb1b0060ee..a117cec6fa 100644 --- a/moveit_setup_assistant/CHANGELOG.rst +++ b/moveit_setup_assistant/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_setup_assistant ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.1 (2019-03-08) +------------------ +* [fix] re-add required build dependencies (`#1373 `_) +* [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) +* Contributors: Isaac I.Y. Saito, Robert Haschke, Yu, Yan + 1.0.0 (2019-02-24) ------------------ * [fix] catkin_lint issues (`#1341 `_) diff --git a/moveit_setup_assistant/package.xml b/moveit_setup_assistant/package.xml index b34039ae44..6db06b5a51 100644 --- a/moveit_setup_assistant/package.xml +++ b/moveit_setup_assistant/package.xml @@ -1,6 +1,6 @@ moveit_setup_assistant - 1.0.0 + 1.0.1 Generates a configuration package that makes it easy to use MoveIt! From c785b5bfb78959c70f786702ebda73334cf2d054 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 12 Mar 2019 21:26:35 +0000 Subject: [PATCH 4/8] Moveit commander planning scene interface fixup (#789) * Make hidden __ methods static and move to end * Remove unused import * Use the apply_planning_scene service to add objects * "X != None" --> "X is not None" * Add arguments asynchronous and service_timeout * Fix PEP style warning (except for line length) --- .../planning_scene_interface.py | 261 ++++++++++-------- 1 file changed, 147 insertions(+), 114 deletions(-) diff --git a/moveit_commander/src/moveit_commander/planning_scene_interface.py b/moveit_commander/src/moveit_commander/planning_scene_interface.py index 80dfbb4034..b38c7f3c29 100644 --- a/moveit_commander/src/moveit_commander/planning_scene_interface.py +++ b/moveit_commander/src/moveit_commander/planning_scene_interface.py @@ -35,11 +35,12 @@ import rospy import conversions -from moveit_msgs.msg import CollisionObject, AttachedCollisionObject +from moveit_msgs.msg import PlanningScene, CollisionObject, AttachedCollisionObject from moveit_ros_planning_interface import _moveit_planning_scene_interface -from geometry_msgs.msg import PoseStamped, Pose, Point +from geometry_msgs.msg import Pose, Point from shape_msgs.msg import SolidPrimitive, Plane, Mesh, MeshTriangle from exception import MoveItCommanderException +from moveit_msgs.srv import ApplyPlanningScene, ApplyPlanningSceneRequest try: from pyassimp import pyassimp @@ -51,128 +52,59 @@ pyassimp = False print("Failed to import pyassimp, see https://github.com/ros-planning/moveit/issues/86 for more info") -# This is going to have more functionality; (feel free to add some!) -# This class will include simple Python code for publishing messages for a planning scene class PlanningSceneInterface(object): """ Simple interface to making updates to a planning scene """ - def __init__(self, ns=''): + def __init__(self, ns='', synchronous=False, service_timeout=5.0): """ Create a planning scene interface; it uses both C++ wrapped methods and scene manipulation topics. """ self._psi = _moveit_planning_scene_interface.PlanningSceneInterface(ns) self._pub_co = rospy.Publisher('/collision_object', CollisionObject, queue_size=100) self._pub_aco = rospy.Publisher('/attached_collision_object', AttachedCollisionObject, queue_size=100) + self.__synchronous = synchronous + if self.__synchronous: + self._apply_planning_scene_diff = rospy.ServiceProxy('apply_planning_scene', ApplyPlanningScene) + self._apply_planning_scene_diff.wait_for_service(service_timeout) - def __make_sphere(self, name, pose, radius): - co = CollisionObject() - co.operation = CollisionObject.ADD - co.id = name - co.header = pose.header - sphere = SolidPrimitive() - sphere.type = SolidPrimitive.SPHERE - sphere.dimensions = [radius] - co.primitives = [sphere] - co.primitive_poses = [pose.pose] - return co + def __submit(self, collision_object, attach=False): + if self.__synchronous: + diff_req = self.__make_planning_scene_diff_req(collision_object) + self._apply_planning_scene_diff.call(diff_req) + else: + if attach: + self._pub_aco.publish(collision_object) + else: + self._pub_co.publish(collision_object) - def add_sphere(self, name, pose, radius = 1): + def add_sphere(self, name, pose, radius=1): """ Add a sphere to the planning scene """ - self._pub_co.publish(self.__make_sphere(name, pose, radius)) - - def __make_cylinder(self, name, pose, height, radius): - co = CollisionObject() - co.operation = CollisionObject.ADD - co.id = name - co.header = pose.header - cylinder = SolidPrimitive() - cylinder.type = SolidPrimitive.CYLINDER - cylinder.dimensions = [height, radius] - co.primitives = [cylinder] - co.primitive_poses = [pose.pose] - return co + co = self.__make_sphere(name, pose, radius) + self.__submit(co, attach=False) def add_cylinder(self, name, pose, height, radius): - """Add a cylinder to the planning scene""" - self._pub_co.publish(self.__make_cylinder(name, pose, height, radius)) - - def __make_box(self, name, pose, size): - co = CollisionObject() - co.operation = CollisionObject.ADD - co.id = name - co.header = pose.header - box = SolidPrimitive() - box.type = SolidPrimitive.BOX - box.dimensions = list(size) - co.primitives = [box] - co.primitive_poses = [pose.pose] - return co - - def __make_mesh(self, name, pose, filename, scale = (1, 1, 1)): - co = CollisionObject() - if pyassimp is False: - raise MoveItCommanderException("Pyassimp needs patch https://launchpadlibrarian.net/319496602/patchPyassim.txt") - scene = pyassimp.load(filename) - if not scene.meshes or len(scene.meshes) == 0: - raise MoveItCommanderException("There are no meshes in the file") - if len(scene.meshes[0].faces) == 0: - raise MoveItCommanderException("There are no faces in the mesh") - co.operation = CollisionObject.ADD - co.id = name - co.header = pose.header - - mesh = Mesh() - first_face = scene.meshes[0].faces[0] - if hasattr(first_face, '__len__'): - for face in scene.meshes[0].faces: - if len(face) == 3: - triangle = MeshTriangle() - triangle.vertex_indices = [face[0], face[1], face[2]] - mesh.triangles.append(triangle) - elif hasattr(first_face, 'indices'): - for face in scene.meshes[0].faces: - if len(face.indices) == 3: - triangle = MeshTriangle() - triangle.vertex_indices = [face.indices[0], - face.indices[1], - face.indices[2]] - mesh.triangles.append(triangle) - else: - raise MoveItCommanderException("Unable to build triangles from mesh due to mesh object structure") - for vertex in scene.meshes[0].vertices: - point = Point() - point.x = vertex[0]*scale[0] - point.y = vertex[1]*scale[1] - point.z = vertex[2]*scale[2] - mesh.vertices.append(point) - co.meshes = [mesh] - co.mesh_poses = [pose.pose] - pyassimp.release(scene) - return co - - def __make_existing(self, name): """ - Create an empty Collision Object, used when the object already exists + Add a cylinder to the planning scene """ - co = CollisionObject() - co.id = name - return co + self._pub_co.publish(self.__make_cylinder(name, pose, height, radius)) - def add_mesh(self, name, pose, filename, size = (1, 1, 1)): + def add_mesh(self, name, pose, filename, size=(1, 1, 1)): """ Add a mesh to the planning scene """ - self._pub_co.publish(self.__make_mesh(name, pose, filename, size)) + co = self.__make_mesh(name, pose, filename, size) + self.__submit(co, attach=False) - def add_box(self, name, pose, size = (1, 1, 1)): + def add_box(self, name, pose, size=(1, 1, 1)): """ Add a box to the planning scene """ - self._pub_co.publish(self.__make_box(name, pose, size)) + co = self.__make_box(name, pose, size) + self.__submit(co, attach=False) - def add_plane(self, name, pose, normal = (0, 0, 1), offset = 0): + def add_plane(self, name, pose, normal=(0, 0, 1), offset=0): """ Add a plane to the planning scene """ co = CollisionObject() co.operation = CollisionObject.ADD @@ -183,11 +115,11 @@ def add_plane(self, name, pose, normal = (0, 0, 1), offset = 0): p.coef.append(offset) co.planes = [p] co.plane_poses = [pose.pose] - self._pub_co.publish(co) + self.__submit(co, attach=False) - def attach_mesh(self, link, name, pose = None, filename = '', size = (1, 1, 1), touch_links = []): + def attach_mesh(self, link, name, pose=None, filename='', size=(1, 1, 1), touch_links=[]): aco = AttachedCollisionObject() - if pose!=None and filename: + if (pose is not None) and filename: aco.object = self.__make_mesh(name, pose, filename, size) else: aco.object = self.__make_existing(name) @@ -195,11 +127,11 @@ def attach_mesh(self, link, name, pose = None, filename = '', size = (1, 1, 1), aco.touch_links = [link] if len(touch_links) > 0: aco.touch_links = touch_links - self._pub_aco.publish(aco) + self.__submit(aco, attach=True) - def attach_box(self, link, name, pose = None, size = (1, 1, 1), touch_links = []): + def attach_box(self, link, name, pose=None, size=(1, 1, 1), touch_links=[]): aco = AttachedCollisionObject() - if pose!=None: + if pose is not None: aco.object = self.__make_box(name, pose, size) else: aco.object = self.__make_existing(name) @@ -208,36 +140,36 @@ def attach_box(self, link, name, pose = None, size = (1, 1, 1), touch_links = [] aco.touch_links = touch_links else: aco.touch_links = [link] - self._pub_aco.publish(aco) + self.__submit(aco, attach=True) - def remove_world_object(self, name = None): + def remove_world_object(self, name=None): """ Remove an object from planning scene, or all if no name is provided """ co = CollisionObject() co.operation = CollisionObject.REMOVE - if name != None: + if name is not None: co.id = name - self._pub_co.publish(co) + self.__submit(co, attach=True) - def remove_attached_object(self, link, name = None): + def remove_attached_object(self, link, name=None): """ Remove an attached object from planning scene, or all objects attached to this link if no name is provided """ aco = AttachedCollisionObject() aco.object.operation = CollisionObject.REMOVE aco.link_name = link - if name != None: + if name is not None: aco.object.id = name - self._pub_aco.publish(aco) + self.__submit(aco, attach=True) - def get_known_object_names(self, with_type = False): + def get_known_object_names(self, with_type=False): """ Get the names of all known objects in the world. If with_type is set to true, only return objects that have a known type. """ return self._psi.get_known_object_names(with_type) - def get_known_object_names_in_roi(self, minx, miny, minz, maxx, maxy, maxz, with_type = False): + def get_known_object_names_in_roi(self, minx, miny, minz, maxx, maxy, maxz, with_type=False): """ Get the names of known objects in the world that are located within a bounding region (specified in the frame reported by get_planning_frame()). If with_type is set to true, only return objects that have a known type. @@ -256,7 +188,7 @@ def get_object_poses(self, object_ids): ops[key] = msg return ops - def get_objects(self, object_ids = []): + def get_objects(self, object_ids=[]): """ Get the objects identified by the given object ids list. If no ids are provided, return all the known objects. """ @@ -268,7 +200,7 @@ def get_objects(self, object_ids = []): objs[key] = msg return objs - def get_attached_objects(self, object_ids = []): + def get_attached_objects(self, object_ids=[]): """ Get the attached objects identified by the given object ids list. If no ids are provided, return all the attached objects. """ @@ -279,3 +211,104 @@ def get_attached_objects(self, object_ids = []): conversions.msg_from_string(msg, ser_aobjs[key]) aobjs[key] = msg return aobjs + + @staticmethod + def __make_existing(name): + """ + Create an empty Collision Object, used when the object already exists + """ + co = CollisionObject() + co.id = name + return co + + @staticmethod + def __make_box(name, pose, size): + co = CollisionObject() + co.operation = CollisionObject.ADD + co.id = name + co.header = pose.header + box = SolidPrimitive() + box.type = SolidPrimitive.BOX + box.dimensions = list(size) + co.primitives = [box] + co.primitive_poses = [pose.pose] + return co + + @staticmethod + def __make_mesh(name, pose, filename, scale=(1, 1, 1)): + co = CollisionObject() + if pyassimp is False: + raise MoveItCommanderException( + "Pyassimp needs patch https://launchpadlibrarian.net/319496602/patchPyassim.txt") + scene = pyassimp.load(filename) + if not scene.meshes or len(scene.meshes) == 0: + raise MoveItCommanderException("There are no meshes in the file") + if len(scene.meshes[0].faces) == 0: + raise MoveItCommanderException("There are no faces in the mesh") + co.operation = CollisionObject.ADD + co.id = name + co.header = pose.header + + mesh = Mesh() + first_face = scene.meshes[0].faces[0] + if hasattr(first_face, '__len__'): + for face in scene.meshes[0].faces: + if len(face) == 3: + triangle = MeshTriangle() + triangle.vertex_indices = [face[0], face[1], face[2]] + mesh.triangles.append(triangle) + elif hasattr(first_face, 'indices'): + for face in scene.meshes[0].faces: + if len(face.indices) == 3: + triangle = MeshTriangle() + triangle.vertex_indices = [face.indices[0], + face.indices[1], + face.indices[2]] + mesh.triangles.append(triangle) + else: + raise MoveItCommanderException("Unable to build triangles from mesh due to mesh object structure") + for vertex in scene.meshes[0].vertices: + point = Point() + point.x = vertex[0] * scale[0] + point.y = vertex[1] * scale[1] + point.z = vertex[2] * scale[2] + mesh.vertices.append(point) + co.meshes = [mesh] + co.mesh_poses = [pose.pose] + pyassimp.release(scene) + return co + + @staticmethod + def __make_sphere(name, pose, radius): + co = CollisionObject() + co.operation = CollisionObject.ADD + co.id = name + co.header = pose.header + sphere = SolidPrimitive() + sphere.type = SolidPrimitive.SPHERE + sphere.dimensions = [radius] + co.primitives = [sphere] + co.primitive_poses = [pose.pose] + return co + + @staticmethod + def __make_cylinder(name, pose, height, radius): + co = CollisionObject() + co.operation = CollisionObject.ADD + co.id = name + co.header = pose.header + cylinder = SolidPrimitive() + cylinder.type = SolidPrimitive.CYLINDER + cylinder.dimensions = [height, radius] + co.primitives = [cylinder] + co.primitive_poses = [pose.pose] + return co + + @staticmethod + def __make_planning_scene_diff_req(collision_object): + scene = PlanningScene() + scene.is_diff = True + scene.world.collision_objects = [collision_object] + planning_scene_diff_req = ApplyPlanningSceneRequest() + planning_scene_diff_req.scene = scene + return planning_scene_diff_req From efaa70c619f963fed3802a613942ee1ab935ad09 Mon Sep 17 00:00:00 2001 From: Bryce Willey Date: Wed, 13 Mar 2019 04:51:14 -0400 Subject: [PATCH 5/8] test_utils: Fixed paths to urdf / srdf files (#1391) PR2 robot requires a different path then Fanuc and Panda. --- .../test/test_fcl_collision_detection.cpp | 2 +- .../test/test_constraint_samplers.cpp | 2 +- .../test/test_constraints.cpp | 2 +- moveit_core/robot_model/test/test.cpp | 2 +- moveit_core/robot_state/test/test_aabb.cpp | 2 +- .../test/test_time_parameterization.cpp | 2 +- .../moveit/utils/robot_model_test_utils.h | 17 ++++++++------ .../utils/src/robot_model_test_utils.cpp | 22 +++++++++++++++++-- 8 files changed, 36 insertions(+), 15 deletions(-) diff --git a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp index 14358da2b5..e606f632a3 100644 --- a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp +++ b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp @@ -57,7 +57,7 @@ class FclCollisionDetectionTester : public testing::Test protected: void SetUp() override { - robot_model_ = moveit::core::loadTestingRobotModel("pr2_description"); + robot_model_ = moveit::core::loadTestingRobotModel("pr2"); robot_model_ok_ = static_cast(robot_model_); kinect_dae_resource_ = "package://moveit_resources/pr2_description/urdf/meshes/sensors/kinect_v0/kinect.dae"; diff --git a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp index 9086e4d69f..bd0d7b956f 100644 --- a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp @@ -72,7 +72,7 @@ class LoadPlanningModelsPr2 : public testing::Test void SetUp() override { - robot_model_ = moveit::core::loadTestingRobotModel("pr2_description"); + robot_model_ = moveit::core::loadTestingRobotModel("pr2"); pr2_kinematics_plugin_right_arm_.reset(new pr2_arm_kinematics::PR2ArmKinematicsPlugin); pr2_kinematics_plugin_right_arm_->initialize(*robot_model_, "right_arm", "torso_lift_link", { "r_wrist_roll_link" }, diff --git a/moveit_core/kinematic_constraints/test/test_constraints.cpp b/moveit_core/kinematic_constraints/test/test_constraints.cpp index 8d79f5e4d8..4c6cd14eba 100644 --- a/moveit_core/kinematic_constraints/test/test_constraints.cpp +++ b/moveit_core/kinematic_constraints/test/test_constraints.cpp @@ -46,7 +46,7 @@ class LoadPlanningModelsPr2 : public testing::Test protected: void SetUp() override { - robot_model_ = moveit::core::loadTestingRobotModel("pr2_description"); + robot_model_ = moveit::core::loadTestingRobotModel("pr2"); } void TearDown() override diff --git a/moveit_core/robot_model/test/test.cpp b/moveit_core/robot_model/test/test.cpp index 70c59eca0c..4ef067e68d 100644 --- a/moveit_core/robot_model/test/test.cpp +++ b/moveit_core/robot_model/test/test.cpp @@ -48,7 +48,7 @@ class LoadPlanningModelsPr2 : public testing::Test protected: void SetUp() override { - robot_model_ = moveit::core::loadTestingRobotModel("pr2_description"); + robot_model_ = moveit::core::loadTestingRobotModel("pr2"); }; void TearDown() override diff --git a/moveit_core/robot_state/test/test_aabb.cpp b/moveit_core/robot_state/test/test_aabb.cpp index a1fba17cd5..35fd1bd2bc 100644 --- a/moveit_core/robot_state/test/test_aabb.cpp +++ b/moveit_core/robot_state/test/test_aabb.cpp @@ -86,7 +86,7 @@ TEST_F(TestAABB, TestPR2) { // Contains a link with mesh geometry that is not centered - robot_state::RobotState pr2_state = this->loadModel("pr2_description"); + robot_state::RobotState pr2_state = this->loadModel("pr2"); const Eigen::Vector3d& extentsBaseFootprint = pr2_state.getLinkModel("base_footprint")->getShapeExtentsAtOrigin(); // values taken from moveit_resources/pr2_description/urdf/robot.xml diff --git a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp index 267694a313..6953ff0499 100644 --- a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp +++ b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp @@ -44,7 +44,7 @@ #include // Static variables used in all tests -moveit::core::RobotModelConstPtr rmodel = moveit::core::loadTestingRobotModel("pr2_description"); +moveit::core::RobotModelConstPtr rmodel = moveit::core::loadTestingRobotModel("pr2"); robot_trajectory::RobotTrajectory trajectory(rmodel, "right_arm"); // Initialize one-joint, 3 points exactly the same. diff --git a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h index 1cc28bc3da..3e6c70f54d 100644 --- a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h +++ b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h @@ -54,23 +54,26 @@ namespace moveit namespace core { /** \brief Loads a robot from moveit_resources. - * \param[in] robot_name The name of the robot package in moveit_resources to load. - * For example, "panda_description", or "fanuc_description". + * \param[in] robot_name The name of the robot in moveit_resources to load. + * This should be the prefix to many of the robot packages. + * For example, "pr2", "panda", or "fanuc". * \returns a RobotModel constructed from robot_name's URDF and SRDF. */ moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& robot_name); /** \brief Loads a URDF Model Interface from moveit_resources. - * \param[in] robot_name The name of the robot package in moveit_resources to load. - * For example, "panda_description", or "fanuc_description". + * \param[in] robot_name The name of the robot in moveit_resources to load. + * This should be the prefix to many of the robot packages. + * For example, "pr2", "panda", or "fanuc". * \returns a ModelInterface constructed from robot_name's URDF. */ urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string& robot_name); /** \brief Loads an SRDF Model from moveit_resources. - * \param[in] robot_name The name of the robot package in moveit_resources to load. - * For example, "panda_description", or "fanuc_description". - * \returns an SRDF Model constructed from robot_names' URDF and SRDF. + * \param[in] robot_name The name of the robot in moveit_resources to load. + * This should be the prefix to many of the robot packages. + * For example, "pr2", "panda", or "fanuc". + * \returns an SRDF Model constructed from robot_name's URDF and SRDF. */ srdf::ModelSharedPtr loadSRDFModel(const std::string& robot_name); diff --git a/moveit_core/utils/src/robot_model_test_utils.cpp b/moveit_core/utils/src/robot_model_test_utils.cpp index c1cbe50530..610ac6a19a 100644 --- a/moveit_core/utils/src/robot_model_test_utils.cpp +++ b/moveit_core/utils/src/robot_model_test_utils.cpp @@ -57,7 +57,16 @@ moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& robot_name) urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string& robot_name) { boost::filesystem::path res_path(MOVEIT_TEST_RESOURCES_DIR); - urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDFFile((res_path / robot_name / "urdf/robot.xml").string()); + std::string urdf_path; + if (robot_name == "pr2") + { + urdf_path = (res_path / "pr2_description/urdf/robot.xml").string(); + } + else + { + urdf_path = (res_path / (robot_name + "_description") / "urdf" / (robot_name + ".urdf")).string(); + } + urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDFFile(urdf_path); if (urdf_model == nullptr) { ROS_ERROR_NAMED(LOGNAME, "Cannot find URDF for %s. Make sure moveit_resources/your robot description is installed", @@ -71,7 +80,16 @@ srdf::ModelSharedPtr loadSRDFModel(const std::string& robot_name) boost::filesystem::path res_path(MOVEIT_TEST_RESOURCES_DIR); urdf::ModelInterfaceSharedPtr urdf_model = loadModelInterface(robot_name); srdf::ModelSharedPtr srdf_model(new srdf::Model()); - srdf_model->initFile(*urdf_model, (res_path / robot_name / "srdf/robot.xml").string()); + std::string srdf_path; + if (robot_name == "pr2") + { + srdf_path = (res_path / "pr2_description/srdf/robot.xml").string(); + } + else + { + srdf_path = (res_path / (robot_name + "_moveit_config") / "config" / (robot_name + ".srdf")).string(); + } + srdf_model->initFile(*urdf_model, srdf_path); return srdf_model; } From 129e8e9a7322e1bccfb2e54cb84732d5e909c21e Mon Sep 17 00:00:00 2001 From: "Yu, Yan" <43195064+RoboticsYY@users.noreply.github.com> Date: Fri, 15 Mar 2019 03:45:42 +0800 Subject: [PATCH 6/8] Apply clang tidy fix to entire code base (Part 2) (#1394) * Conform class name to `CamelCase` * Conform member method name to `camelBack` * Exceptions to method name * Conform local variable name to `lower_case` part 1 * Conform local variable name to `lower_case` part 2 * Conform local variable name to `lower_case` part 3 * Conform local variable name to `lower_case` part 4 * Local static variable to `lower_case` * Local variable manual fix * Exceptions to local variable name * Conform static const variable name to `UPPER_CASE` * Conform global variable name to `UPPER_CASE` * Conform static const member variable to `UPPER_CASE` * clang-format * Travis: mandatory clang-tidy-check * Catch up most recent changes * Update .clang-tidy * fixup! Conform static const variable name to `UPPER_CASE` --- .clang-tidy | 1 + .travis.yml | 4 - .../collision_detector_allocator_allvalid.h | 2 +- .../collision_detector_allocator.h | 2 +- .../src/allvalid/collision_world_allvalid.cpp | 2 +- .../collision_detector_allocator_fcl.h | 2 +- .../src/collision_world_fcl.cpp | 2 +- ...lision_detector_allocator_distance_field.h | 2 +- .../collision_detector_allocator_hybrid.h | 2 +- .../src/collision_world_distance_field.cpp | 2 +- .../src/collision_world_hybrid.cpp | 2 +- .../test/test_distance_field.cpp | 78 ++--- .../distance_field/test/test_voxel_grid.cpp | 30 +- .../test/robot_state_benchmark.cpp | 18 +- moveit_core/robot_state/test/test_aabb.cpp | 35 ++- .../time_optimal_trajectory_generation.cpp | 28 +- ...est_time_optimal_trajectory_generation.cpp | 30 +- .../test/test_time_parameterization.cpp | 36 +-- .../src/chainiksolver_vel_mimic_svd.cpp | 9 +- .../src/kdl_kinematics_plugin.cpp | 1 + .../test/test_kinematics_plugin.cpp | 22 +- .../src/chomp_optimizer.cpp | 32 +- .../src/chomp_trajectory.cpp | 24 +- .../src/chomp_optimizer_adapter.cpp | 4 +- .../src/constraints_library.cpp | 6 +- .../src/planning_context_manager.cpp | 8 +- .../src/moveit_fake_controllers.cpp | 16 +- .../benchmarks/src/BenchmarkExecutor.cpp | 14 +- .../src/pick_place_action_capability.cpp | 36 +-- .../src/pick_place_action_capability.h | 24 +- .../src/approach_and_translate_stage.cpp | 10 +- .../pick_place/src/manipulation_pipeline.cpp | 8 +- .../pick_place/src/pick_place_params.cpp | 4 +- .../move_action_capability.cpp | 12 +- .../move_action_capability.h | 8 +- .../src/depth_image_octomap_updater.cpp | 24 +- .../include/moveit/mesh_filter/mesh_filter.h | 6 +- .../moveit/mesh_filter/stereo_camera_model.h | 10 +- .../perception/mesh_filter/src/gl_mesh.cpp | 8 +- .../mesh_filter/src/gl_renderer.cpp | 132 ++++---- .../mesh_filter/src/mesh_filter_base.cpp | 6 +- .../mesh_filter/src/sensor_model.cpp | 8 +- .../mesh_filter/src/stereo_camera_model.cpp | 37 +-- .../mesh_filter/test/mesh_filter_test.cpp | 20 +- .../src/occupancy_map_monitor.cpp | 6 +- .../src/shape_mask.cpp | 4 +- .../src/pointcloud_octomap_updater.cpp | 10 +- .../semantic_world/src/semantic_world.cpp | 92 +++--- .../src/visualize_robot_collision_volume.cpp | 8 +- .../src/add_time_optimal_parameterization.cpp | 11 +- .../src/planning_scene_monitor.cpp | 2 +- .../src/trajectory_execution_manager.cpp | 10 +- .../src/move_group_interface.cpp | 8 +- .../src/wrap_python_move_group.cpp | 202 ++++++------ .../wrap_python_planning_scene_interface.cpp | 20 +- .../src/wrap_python_robot_interface.cpp | 54 ++-- .../src/robot_interaction.cpp | 12 +- .../src/motion_planning_display.cpp | 10 +- .../src/motion_planning_frame.cpp | 16 +- .../motion_planning_frame_manipulation.cpp | 8 +- .../src/motion_planning_frame_objects.cpp | 76 ++--- .../src/motion_planning_frame_planning.cpp | 8 +- .../src/motion_planning_frame_states.cpp | 12 +- .../src/planning_scene_display.cpp | 8 +- .../src/robot_state_display.cpp | 4 +- .../src/octomap_render.cpp | 48 +-- .../warehouse/src/moveit_message_storage.cpp | 8 +- .../warehouse/warehouse/src/save_as_text.cpp | 38 +-- .../src/setup_assistant_main.cpp | 4 +- .../src/tools/collision_linear_model.cpp | 18 +- .../src/tools/collision_matrix_model.cpp | 8 +- .../src/tools/compute_default_collisions.cpp | 27 +- .../src/tools/moveit_config_data.cpp | 292 +++++++++--------- .../src/widgets/author_information_widget.cpp | 8 +- .../src/widgets/author_information_widget.h | 4 +- .../widgets/configuration_files_widget.cpp | 20 +- .../src/widgets/default_collisions_widget.cpp | 4 +- .../src/widgets/double_list_widget.cpp | 16 +- .../src/widgets/end_effectors_widget.cpp | 10 +- .../src/widgets/group_edit_widget.cpp | 6 +- .../src/widgets/header_widget.cpp | 4 +- .../src/widgets/header_widget.h | 2 +- .../src/widgets/navigation_widget.cpp | 32 +- .../src/widgets/perception_widget.cpp | 6 +- .../src/widgets/planning_groups_widget.cpp | 36 +-- .../src/widgets/robot_poses_widget.cpp | 10 +- .../src/widgets/ros_controllers_widget.cpp | 4 +- .../src/widgets/start_screen_widget.cpp | 10 +- .../src/widgets/virtual_joints_widget.cpp | 26 +- .../test/moveit_config_data_test.cpp | 42 +-- 90 files changed, 1021 insertions(+), 1010 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index 238bcfc815..e63508cbba 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -11,6 +11,7 @@ Checks: '-*, readability-redundant-string-cstr, readability-simplify-boolean-expr, readability-container-size-empty, + readability-identifier-naming, ' HeaderFilterRegex: '' AnalyzeTemporaryDtors: false diff --git a/.travis.yml b/.travis.yml index aec3649667..edb4edffe3 100644 --- a/.travis.yml +++ b/.travis.yml @@ -34,10 +34,6 @@ matrix: # Add a separate config to the matrix, using clang as compiler BEFORE_DOCKER_SCRIPT="source moveit_kinematics/test/test_ikfast_plugins.sh" CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unused-parameter -Wno-unused-function -Wno-overloaded-virtual" - fast_finish: true # finish, even if allow-failure-tests still running - allow_failures: - - env: TEST=clang-tidy-fix # need to fix clang-tidy issues - before_script: - git clone -q --depth=1 https://github.com/ros-planning/moveit_ci.git .moveit_ci diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h index 51754bfef5..549a6d6990 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h @@ -49,7 +49,7 @@ class CollisionDetectorAllocatorAllValid CollisionDetectorAllocatorAllValid> { public: - static const std::string NAME_; // defined in collision_world_allvalid.cpp + static const std::string NAME; // defined in collision_world_allvalid.cpp }; } diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h index c2535b8372..aed7eac019 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h @@ -78,7 +78,7 @@ class CollisionDetectorAllocatorTemplate : public CollisionDetectorAllocator public: const std::string& getName() const override { - return CollisionDetectorAllocatorType::NAME_; + return CollisionDetectorAllocatorType::NAME; } CollisionWorldPtr allocateWorld(const WorldPtr& world) const override diff --git a/moveit_core/collision_detection/src/allvalid/collision_world_allvalid.cpp b/moveit_core/collision_detection/src/allvalid/collision_world_allvalid.cpp index 9241bd8544..023f308a62 100644 --- a/moveit_core/collision_detection/src/allvalid/collision_world_allvalid.cpp +++ b/moveit_core/collision_detection/src/allvalid/collision_world_allvalid.cpp @@ -146,4 +146,4 @@ void collision_detection::CollisionWorldAllValid::distanceWorld(const collision_ } #include -const std::string collision_detection::CollisionDetectorAllocatorAllValid::NAME_("ALL_VALID"); +const std::string collision_detection::CollisionDetectorAllocatorAllValid::NAME("ALL_VALID"); diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h index dbfcadae30..6b7ec1ce52 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h @@ -48,7 +48,7 @@ class CollisionDetectorAllocatorFCL : public CollisionDetectorAllocatorTemplate { public: - static const std::string NAME_; // defined in collision_world_fcl.cpp + static const std::string NAME; // defined in collision_world_fcl.cpp }; } diff --git a/moveit_core/collision_detection_fcl/src/collision_world_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_world_fcl.cpp index 2fcb050712..8d6eb392f9 100644 --- a/moveit_core/collision_detection_fcl/src/collision_world_fcl.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_world_fcl.cpp @@ -54,7 +54,7 @@ namespace collision_detection { -const std::string CollisionDetectorAllocatorFCL::NAME_("FCL"); +const std::string CollisionDetectorAllocatorFCL::NAME("FCL"); CollisionWorldFCL::CollisionWorldFCL() : CollisionWorld() { diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h index bc82ef22b5..53c71b9f35 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h @@ -49,7 +49,7 @@ class CollisionDetectorAllocatorDistanceField CollisionDetectorAllocatorDistanceField> { public: - static const std::string NAME_; // defined in collision_world_distance_field.cpp + static const std::string NAME; // defined in collision_world_distance_field.cpp }; } diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h index 8acdfa5bac..6eb3277ee7 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h @@ -49,7 +49,7 @@ class CollisionDetectorAllocatorHybrid CollisionDetectorAllocatorHybrid> { public: - static const std::string NAME_; // defined in collision_world_hybrid.cpp + static const std::string NAME; // defined in collision_world_hybrid.cpp }; } diff --git a/moveit_core/collision_distance_field/src/collision_world_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_world_distance_field.cpp index 2e5c8fccb1..478c1e00ae 100644 --- a/moveit_core/collision_distance_field/src/collision_world_distance_field.cpp +++ b/moveit_core/collision_distance_field/src/collision_world_distance_field.cpp @@ -566,4 +566,4 @@ CollisionWorldDistanceField::DistanceFieldCacheEntryPtr CollisionWorldDistanceFi } // namespace collision_detection #include -const std::string collision_detection::CollisionDetectorAllocatorDistanceField::NAME_("DISTANCE_FIELD"); +const std::string collision_detection::CollisionDetectorAllocatorDistanceField::NAME("DISTANCE_FIELD"); diff --git a/moveit_core/collision_distance_field/src/collision_world_hybrid.cpp b/moveit_core/collision_distance_field/src/collision_world_hybrid.cpp index 84cd1ba8bd..551937cd6a 100644 --- a/moveit_core/collision_distance_field/src/collision_world_hybrid.cpp +++ b/moveit_core/collision_distance_field/src/collision_world_hybrid.cpp @@ -157,4 +157,4 @@ void CollisionWorldHybrid::getAllCollisions(const CollisionRequest& req, Collisi } // namespace collision_detection #include -const std::string collision_detection::CollisionDetectorAllocatorHybrid::NAME_("HYBRID"); +const std::string collision_detection::CollisionDetectorAllocatorHybrid::NAME("HYBRID"); diff --git a/moveit_core/distance_field/test/test_distance_field.cpp b/moveit_core/distance_field/test/test_distance_field.cpp index 1198068c93..7417915298 100644 --- a/moveit_core/distance_field/test/test_distance_field.cpp +++ b/moveit_core/distance_field/test/test_distance_field.cpp @@ -48,16 +48,16 @@ using namespace distance_field; -static const double width = 1.0; -static const double height = 1.0; -static const double depth = 1.0; -static const double resolution = 0.1; +static const double WIDTH = 1.0; +static const double HEIGHT = 1.0; +static const double DEPTH = 1.0; +static const double RESOLUTION = 0.1; static const double ORIGIN_X = 0.0; static const double ORIGIN_Y = 0.0; static const double ORIGIN_Z = 0.0; -static const double max_dist = 0.3; +static const double MAX_DIST = 0.3; -static const Eigen::Vector3d point1(0.1, 0.0, 0.0); +static const Eigen::Vector3d POINT1(0.1, 0.0, 0.0); static const Eigen::Vector3d POINT2(0.0, 0.1, 0.2); static const Eigen::Vector3d POINT3(0.4, 0.0, 0.0); @@ -328,27 +328,27 @@ void check_distance_field(const PropagationDistanceField& df, const EigenSTL::ve TEST(TestPropagationDistanceField, TestAddRemovePoints) { - PropagationDistanceField df(width, height, depth, resolution, ORIGIN_X, ORIGIN_Y, ORIGIN_Z, max_dist); + PropagationDistanceField df(WIDTH, HEIGHT, DEPTH, RESOLUTION, ORIGIN_X, ORIGIN_Y, ORIGIN_Z, MAX_DIST); // Check size - int numX = df.getXNumCells(); - int numY = df.getYNumCells(); - int numZ = df.getZNumCells(); + int num_x = df.getXNumCells(); + int num_y = df.getYNumCells(); + int num_z = df.getZNumCells(); - EXPECT_EQ(numX, (int)(width / resolution + 0.5)); - EXPECT_EQ(numY, (int)(height / resolution + 0.5)); - EXPECT_EQ(numZ, (int)(depth / resolution + 0.5)); + EXPECT_EQ(num_x, (int)(WIDTH / RESOLUTION + 0.5)); + EXPECT_EQ(num_y, (int)(HEIGHT / RESOLUTION + 0.5)); + EXPECT_EQ(num_z, (int)(DEPTH / RESOLUTION + 0.5)); // getting a bad point double tgx, tgy, tgz; bool in_bounds; - EXPECT_NEAR(df.getDistance(1000.0, 1000.0, 1000.0), max_dist, .0001); - EXPECT_NEAR(df.getDistanceGradient(1000.0, 1000.0, 1000.0, tgx, tgy, tgz, in_bounds), max_dist, .0001); + EXPECT_NEAR(df.getDistance(1000.0, 1000.0, 1000.0), MAX_DIST, .0001); + EXPECT_NEAR(df.getDistanceGradient(1000.0, 1000.0, 1000.0, tgx, tgy, tgz, in_bounds), MAX_DIST, .0001); EXPECT_FALSE(in_bounds); // Add points to the grid EigenSTL::vector_Vector3d points; - points.push_back(point1); + points.push_back(POINT1); points.push_back(POINT2); ROS_INFO_NAMED("distance_field", "Adding %zu points", points.size()); df.addPointsToField(points); @@ -359,12 +359,12 @@ TEST(TestPropagationDistanceField, TestAddRemovePoints) points.push_back(POINT2); points.push_back(POINT3); EigenSTL::vector_Vector3d old_points; - old_points.push_back(point1); + old_points.push_back(POINT1); df.updatePointsInField(old_points, points); // std::cout << "One removal, one addition" << std::endl; // print(df, numX, numY, numZ); // printNeg(df, numX, numY, numZ); - check_distance_field(df, points, numX, numY, numZ, false); + check_distance_field(df, points, num_x, num_y, num_z, false); // Remove points.clear(); @@ -372,12 +372,12 @@ TEST(TestPropagationDistanceField, TestAddRemovePoints) df.removePointsFromField(points); points.clear(); points.push_back(POINT3); - check_distance_field(df, points, numX, numY, numZ, false); + check_distance_field(df, points, num_x, num_y, num_z, false); // now testing gradient calls df.reset(); points.clear(); - points.push_back(point1); + points.push_back(POINT1); df.addPointsToField(points); bool first = true; for (int z = 1; z < df.getZNumCells() - 1; z++) @@ -394,7 +394,7 @@ TEST(TestPropagationDistanceField, TestAddRemovePoints) double dist_grad = df.getDistanceGradient(wx, wy, wz, grad.x(), grad.y(), grad.z(), grad_in_bounds); ASSERT_TRUE(grad_in_bounds) << x << " " << y << " " << z; ASSERT_NEAR(dist, dist_grad, .0001); - if (dist > 0 && dist < max_dist) + if (dist > 0 && dist < MAX_DIST) { double xscale = grad.x() / grad.norm(); double yscale = grad.y() / grad.norm(); @@ -412,13 +412,13 @@ TEST(TestPropagationDistanceField, TestAddRemovePoints) std::cout << "Grad " << grad.x() << " " << grad.y() << " " << grad.z() << " comp " << comp_x << " " << comp_y << " " << comp_z << std::endl; } - ASSERT_NEAR(comp_x, point1.x(), resolution) << dist << x << " " << y << " " << z << " " << grad.x() << " " + ASSERT_NEAR(comp_x, POINT1.x(), RESOLUTION) << dist << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() << " " << grad.z() << " " << xscale << " " << yscale << " " << zscale << std::endl; - ASSERT_NEAR(comp_y, point1.y(), resolution) << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() + ASSERT_NEAR(comp_y, POINT1.y(), RESOLUTION) << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() << " " << grad.z() << " " << xscale << " " << yscale << " " << zscale << std::endl; - ASSERT_NEAR(comp_z, point1.z(), resolution) << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() + ASSERT_NEAR(comp_z, POINT1.z(), RESOLUTION) << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() << " " << grad.z() << " " << xscale << " " << yscale << " " << zscale << std::endl; } @@ -430,16 +430,16 @@ TEST(TestPropagationDistanceField, TestAddRemovePoints) TEST(TestSignedPropagationDistanceField, TestSignedAddRemovePoints) { - PropagationDistanceField df(width, height, depth, resolution, ORIGIN_X, ORIGIN_Y, ORIGIN_Z, max_dist, true); + PropagationDistanceField df(WIDTH, HEIGHT, DEPTH, RESOLUTION, ORIGIN_X, ORIGIN_Y, ORIGIN_Z, MAX_DIST, true); // Check size - int numX = df.getXNumCells(); - int numY = df.getYNumCells(); - int numZ = df.getZNumCells(); + int num_x = df.getXNumCells(); + int num_y = df.getYNumCells(); + int num_z = df.getZNumCells(); - EXPECT_EQ(numX, (int)(width / resolution + 0.5)); - EXPECT_EQ(numY, (int)(height / resolution + 0.5)); - EXPECT_EQ(numZ, (int)(depth / resolution + 0.5)); + EXPECT_EQ(num_x, (int)(WIDTH / RESOLUTION + 0.5)); + EXPECT_EQ(num_y, (int)(HEIGHT / RESOLUTION + 0.5)); + EXPECT_EQ(num_z, (int)(DEPTH / RESOLUTION + 0.5)); // Error checking // print(df, numX, numY, numZ); @@ -485,7 +485,7 @@ TEST(TestSignedPropagationDistanceField, TestSignedAddRemovePoints) // printNeg(df, numX, numY, numZ); // testing equality with initial add of points without the center point - PropagationDistanceField test_df(width, height, depth, resolution, ORIGIN_X, ORIGIN_Y, ORIGIN_Z, max_dist, true); + PropagationDistanceField test_df(WIDTH, HEIGHT, DEPTH, RESOLUTION, ORIGIN_X, ORIGIN_Y, ORIGIN_Z, MAX_DIST, true); EigenSTL::vector_Vector3d test_points; for (unsigned int i = 0; i < points.size(); i++) { @@ -497,7 +497,7 @@ TEST(TestSignedPropagationDistanceField, TestSignedAddRemovePoints) test_df.addPointsToField(test_points); ASSERT_TRUE(areDistanceFieldsDistancesEqual(df, test_df)); - PropagationDistanceField gradient_df(width, height, depth, resolution, ORIGIN_X, ORIGIN_Y, ORIGIN_Z, max_dist, true); + PropagationDistanceField gradient_df(WIDTH, HEIGHT, DEPTH, RESOLUTION, ORIGIN_X, ORIGIN_Y, ORIGIN_Z, MAX_DIST, true); shapes::Sphere sphere(.25); @@ -600,7 +600,7 @@ TEST(TestSignedPropagationDistanceField, TestSignedAddRemovePoints) TEST(TestSignedPropagationDistanceField, TestShape) { - PropagationDistanceField df(width, height, depth, resolution, ORIGIN_X, ORIGIN_Y, ORIGIN_Z, max_dist, true); + PropagationDistanceField df(WIDTH, HEIGHT, DEPTH, RESOLUTION, ORIGIN_X, ORIGIN_Y, ORIGIN_Z, MAX_DIST, true); int num_x = df.getXNumCells(); int num_y = df.getYNumCells(); @@ -616,7 +616,7 @@ TEST(TestSignedPropagationDistanceField, TestShape) bodies::Body* body = bodies::createBodyFromShape(&sphere); body->setPose(p); EigenSTL::vector_Vector3d point_vec; - findInternalPointsConvex(*body, resolution, point_vec); + findInternalPointsConvex(*body, RESOLUTION, point_vec); delete body; check_distance_field(df, point_vec, num_x, num_y, num_z, true); @@ -631,7 +631,7 @@ TEST(TestSignedPropagationDistanceField, TestShape) body->setPose(np); EigenSTL::vector_Vector3d point_vec_2; - findInternalPointsConvex(*body, resolution, point_vec_2); + findInternalPointsConvex(*body, RESOLUTION, point_vec_2); delete body; EigenSTL::vector_Vector3d point_vec_union = point_vec_2; point_vec_union.insert(point_vec_union.end(), point_vec.begin(), point_vec.end()); @@ -643,7 +643,7 @@ TEST(TestSignedPropagationDistanceField, TestShape) check_distance_field(df, point_vec_2, num_x, num_y, num_z, true); // should be equivalent to just adding second shape - PropagationDistanceField test_df(width, height, depth, resolution, ORIGIN_X, ORIGIN_Y, ORIGIN_Z, max_dist, true); + PropagationDistanceField test_df(WIDTH, HEIGHT, DEPTH, RESOLUTION, ORIGIN_X, ORIGIN_Y, ORIGIN_Z, MAX_DIST, true); test_df.addShapeToField(&sphere, np); ASSERT_TRUE(areDistanceFieldsDistancesEqual(df, test_df)); } @@ -883,10 +883,10 @@ TEST(TestSignedPropagationDistanceField, TestOcTree) TEST(TestSignedPropagationDistanceField, TestReadWrite) { - PropagationDistanceField small_df(width, height, depth, resolution, ORIGIN_X, ORIGIN_Y, ORIGIN_Z, max_dist); + PropagationDistanceField small_df(WIDTH, HEIGHT, DEPTH, RESOLUTION, ORIGIN_X, ORIGIN_Y, ORIGIN_Z, MAX_DIST); EigenSTL::vector_Vector3d points; - points.push_back(point1); + points.push_back(POINT1); points.push_back(POINT2); points.push_back(POINT3); small_df.addPointsToField(points); diff --git a/moveit_core/distance_field/test/test_voxel_grid.cpp b/moveit_core/distance_field/test/test_voxel_grid.cpp index 477728cb22..7538583615 100644 --- a/moveit_core/distance_field/test/test_voxel_grid.cpp +++ b/moveit_core/distance_field/test/test_voxel_grid.cpp @@ -47,22 +47,22 @@ TEST(TestVoxelGrid, TestReadWrite) int def = -100; VoxelGrid vg(0.02, 0.02, 0.02, 0.01, 0, 0, 0, def); - int numX = vg.getNumCells(DIM_X); - int numY = vg.getNumCells(DIM_Y); - int numZ = vg.getNumCells(DIM_Z); + int num_x = vg.getNumCells(DIM_X); + int num_y = vg.getNumCells(DIM_Y); + int num_z = vg.getNumCells(DIM_Z); // Check dimensions - EXPECT_EQ(numX, 2); - EXPECT_EQ(numY, 2); - EXPECT_EQ(numZ, 2); + EXPECT_EQ(num_x, 2); + EXPECT_EQ(num_y, 2); + EXPECT_EQ(num_z, 2); // check initial values vg.reset(0); i = 0; - for (int x = 0; x < numX; x++) - for (int y = 0; y < numY; y++) - for (int z = 0; z < numZ; z++) + for (int x = 0; x < num_x; x++) + for (int y = 0; y < num_y; y++) + for (int z = 0; z < num_z; z++) { EXPECT_EQ(vg.getCell(x, y, z), 0); i++; @@ -76,9 +76,9 @@ TEST(TestVoxelGrid, TestReadWrite) // Set values i = 0; - for (int x = 0; x < numX; x++) - for (int y = 0; y < numY; y++) - for (int z = 0; z < numZ; z++) + for (int x = 0; x < num_x; x++) + for (int y = 0; y < num_y; y++) + for (int z = 0; z < num_z; z++) { vg.getCell(x, y, z) = i; i++; @@ -86,9 +86,9 @@ TEST(TestVoxelGrid, TestReadWrite) // check reset values i = 0; - for (int x = 0; x < numX; x++) - for (int y = 0; y < numY; y++) - for (int z = 0; z < numZ; z++) + for (int x = 0; x < num_x; x++) + for (int y = 0; y < num_y; y++) + for (int z = 0; z < num_z; z++) { EXPECT_EQ(i, vg.getCell(x, y, z)); i++; diff --git a/moveit_core/robot_state/test/robot_state_benchmark.cpp b/moveit_core/robot_state/test/robot_state_benchmark.cpp index b50f5a2b24..cd4bdbf993 100644 --- a/moveit_core/robot_state/test/robot_state_benchmark.cpp +++ b/moveit_core/robot_state/test/robot_state_benchmark.cpp @@ -109,21 +109,21 @@ TEST_F(Timing, stateUpdate) TEST_F(Timing, multiply) { - size_t RUNS = 1e7; + size_t runs = 1e7; double gold_standard = 0; { ScopedTimer t("Eigen::Affine * Eigen::Matrix: ", &gold_standard); - for (size_t i = 0; i < RUNS; ++i) + for (size_t i = 0; i < runs; ++i) transforms_[result_idx_].affine().noalias() = transforms_[input_idx_].affine() * transforms_[input_idx_].matrix(); } { ScopedTimer t("Eigen::Matrix * Eigen::Matrix: ", &gold_standard); - for (size_t i = 0; i < RUNS; ++i) + for (size_t i = 0; i < runs; ++i) transforms_[result_idx_].matrix().noalias() = transforms_[input_idx_].matrix() * transforms_[input_idx_].matrix(); } { ScopedTimer t("Eigen::Isometry * Eigen::Isometry: ", &gold_standard); - for (size_t i = 0; i < RUNS; ++i) + for (size_t i = 0; i < runs; ++i) transforms_[result_idx_] = transforms_[input_idx_] * transforms_[input_idx_]; } } @@ -132,27 +132,27 @@ TEST_F(Timing, inverse) { EigenSTL::vector_Affine3d affine(1); affine[0].matrix() = transforms_[input_idx_].matrix(); - size_t RUNS = 1e7; + size_t runs = 1e7; double gold_standard = 0; { ScopedTimer t("Isometry3d::inverse(): ", &gold_standard); - for (size_t i = 0; i < RUNS; ++i) + for (size_t i = 0; i < runs; ++i) transforms_[result_idx_] = transforms_[input_idx_].inverse(); } volatile size_t input_idx = 0; { ScopedTimer t("Affine3d::inverse(Eigen::Isometry): ", &gold_standard); - for (size_t i = 0; i < RUNS; ++i) + for (size_t i = 0; i < runs; ++i) transforms_[result_idx_].affine().noalias() = affine[input_idx].inverse(Eigen::Isometry).affine(); } { ScopedTimer t("Affine3d::inverse(): ", &gold_standard); - for (size_t i = 0; i < RUNS; ++i) + for (size_t i = 0; i < runs; ++i) transforms_[result_idx_].affine().noalias() = affine[input_idx].inverse().affine(); } { ScopedTimer t("Matrix4d::inverse(): ", &gold_standard); - for (size_t i = 0; i < RUNS; ++i) + for (size_t i = 0; i < runs; ++i) transforms_[result_idx_].matrix().noalias() = affine[input_idx].matrix().inverse(); } } diff --git a/moveit_core/robot_state/test/test_aabb.cpp b/moveit_core/robot_state/test/test_aabb.cpp index 35fd1bd2bc..9f13d7d2f7 100644 --- a/moveit_core/robot_state/test/test_aabb.cpp +++ b/moveit_core/robot_state/test/test_aabb.cpp @@ -88,27 +88,28 @@ TEST_F(TestAABB, TestPR2) robot_state::RobotState pr2_state = this->loadModel("pr2"); - const Eigen::Vector3d& extentsBaseFootprint = pr2_state.getLinkModel("base_footprint")->getShapeExtentsAtOrigin(); + const Eigen::Vector3d& extents_base_footprint = pr2_state.getLinkModel("base_footprint")->getShapeExtentsAtOrigin(); // values taken from moveit_resources/pr2_description/urdf/robot.xml - EXPECT_NEAR(extentsBaseFootprint[0], 0.001, 1e-4); - EXPECT_NEAR(extentsBaseFootprint[1], 0.001, 1e-4); - EXPECT_NEAR(extentsBaseFootprint[2], 0.001, 1e-4); + EXPECT_NEAR(extents_base_footprint[0], 0.001, 1e-4); + EXPECT_NEAR(extents_base_footprint[1], 0.001, 1e-4); + EXPECT_NEAR(extents_base_footprint[2], 0.001, 1e-4); - const Eigen::Vector3d& offsetBaseFootprint = pr2_state.getLinkModel("base_footprint")->getCenteredBoundingBoxOffset(); - EXPECT_NEAR(offsetBaseFootprint[0], 0.0, 1e-4); - EXPECT_NEAR(offsetBaseFootprint[1], 0.0, 1e-4); - EXPECT_NEAR(offsetBaseFootprint[2], 0.071, 1e-4); + const Eigen::Vector3d& offset_base_footprint = + pr2_state.getLinkModel("base_footprint")->getCenteredBoundingBoxOffset(); + EXPECT_NEAR(offset_base_footprint[0], 0.0, 1e-4); + EXPECT_NEAR(offset_base_footprint[1], 0.0, 1e-4); + EXPECT_NEAR(offset_base_footprint[2], 0.071, 1e-4); - const Eigen::Vector3d& extentsBaseLink = pr2_state.getLinkModel("base_link")->getShapeExtentsAtOrigin(); + const Eigen::Vector3d& extents_base_link = pr2_state.getLinkModel("base_link")->getShapeExtentsAtOrigin(); // values computed from moveit_resources/pr2_description/urdf/meshes/base_v0/base_L.stl in e.g. Meshlab - EXPECT_NEAR(extentsBaseLink[0], 0.668242, 1e-4); - EXPECT_NEAR(extentsBaseLink[1], 0.668242, 1e-4); - EXPECT_NEAR(extentsBaseLink[2], 0.656175, 1e-4); - - const Eigen::Vector3d& offsetBaseLink = pr2_state.getLinkModel("base_link")->getCenteredBoundingBoxOffset(); - EXPECT_NEAR(offsetBaseLink[0], 0.0, 1e-4); - EXPECT_NEAR(offsetBaseLink[1], 0.0, 1e-4); - EXPECT_NEAR(offsetBaseLink[2], 0.656175 / 2, 1e-4); // The 3D mesh isn't centered, but is whole above z axis + EXPECT_NEAR(extents_base_link[0], 0.668242, 1e-4); + EXPECT_NEAR(extents_base_link[1], 0.668242, 1e-4); + EXPECT_NEAR(extents_base_link[2], 0.656175, 1e-4); + + const Eigen::Vector3d& offset_base_link = pr2_state.getLinkModel("base_link")->getCenteredBoundingBoxOffset(); + EXPECT_NEAR(offset_base_link[0], 0.0, 1e-4); + EXPECT_NEAR(offset_base_link[1], 0.0, 1e-4); + EXPECT_NEAR(offset_base_link[2], 0.656175 / 2, 1e-4); // The 3D mesh isn't centered, but is whole above z axis std::vector pr2_aabb; pr2_state.computeAABB(pr2_aabb); 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 b9a1e09416..594b851489 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -59,29 +59,29 @@ class LinearPathSegment : public PathSegment { } - Eigen::VectorXd getConfig(double s) const + Eigen::VectorXd getConfig(double s) const override { s /= length_; s = std::max(0.0, std::min(1.0, s)); return (1.0 - s) * start_ + s * end_; } - Eigen::VectorXd getTangent(double /* s */) const + Eigen::VectorXd getTangent(double /* s */) const override { return (end_ - start_) / length_; } - Eigen::VectorXd getCurvature(double /* s */) const + Eigen::VectorXd getCurvature(double /* s */) const override { return Eigen::VectorXd::Zero(start_.size()); } - std::list getSwitchingPoints() const + std::list getSwitchingPoints() const override { return std::list(); } - LinearPathSegment* clone() const + LinearPathSegment* clone() const override { return new LinearPathSegment(*this); } @@ -138,25 +138,25 @@ class CircularPathSegment : public PathSegment y = start_direction; } - Eigen::VectorXd getConfig(double s) const + Eigen::VectorXd getConfig(double s) const override { const double angle = s / radius; return center + radius * (x * cos(angle) + y * sin(angle)); } - Eigen::VectorXd getTangent(double s) const + Eigen::VectorXd getTangent(double s) const override { const double angle = s / radius; return -x * sin(angle) + y * cos(angle); } - Eigen::VectorXd getCurvature(double s) const + Eigen::VectorXd getCurvature(double s) const override { const double angle = s / radius; return -1.0 / radius * (x * cos(angle) + y * sin(angle)); } - std::list getSwitchingPoints() const + std::list getSwitchingPoints() const override { std::list switching_points; const double dim = x.size(); @@ -177,7 +177,7 @@ class CircularPathSegment : public PathSegment return switching_points; } - CircularPathSegment* clone() const + CircularPathSegment* clone() const override { return new CircularPathSegment(*this); } @@ -363,7 +363,7 @@ Trajectory::Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, co } } -Trajectory::~Trajectory(void) +Trajectory::~Trajectory() { } @@ -719,12 +719,12 @@ double Trajectory::getAccelerationMaxPathVelocity(double path_pos) const { if (config_deriv[j] != 0.0) { - double A_ij = config_deriv2[i] / config_deriv[i] - config_deriv2[j] / config_deriv[j]; - if (A_ij != 0.0) + double a_ij = config_deriv2[i] / config_deriv[i] - config_deriv2[j] / config_deriv[j]; + if (a_ij != 0.0) { max_path_velocity = std::min(max_path_velocity, sqrt((max_acceleration_[i] / std::abs(config_deriv[i]) + max_acceleration_[j] / std::abs(config_deriv[j])) / - std::abs(A_ij))); + std::abs(a_ij))); } } } diff --git a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp index 807878a88c..d05a423ce9 100644 --- a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp @@ -52,12 +52,12 @@ TEST(time_optimal_trajectory_generation, test1) waypoint << 1423.0, 985.000244140625, 2126.0, 0.0; waypoints.push_back(waypoint); - Eigen::VectorXd maxVelocities(4); - maxVelocities << 1.3, 0.67, 0.67, 0.5; - Eigen::VectorXd maxAccelerations(4); - maxAccelerations << 0.00249, 0.00249, 0.00249, 0.00249; + Eigen::VectorXd max_velocities(4); + max_velocities << 1.3, 0.67, 0.67, 0.5; + Eigen::VectorXd max_accelerations(4); + max_accelerations << 0.00249, 0.00249, 0.00249, 0.00249; - Trajectory trajectory(Path(waypoints, 100.0), maxVelocities, maxAccelerations, 10.0); + Trajectory trajectory(Path(waypoints, 100.0), max_velocities, max_accelerations, 10.0); EXPECT_TRUE(trajectory.isValid()); EXPECT_DOUBLE_EQ(40.080256821829849, trajectory.getDuration()); @@ -90,12 +90,12 @@ TEST(time_optimal_trajectory_generation, test2) waypoint << 452.5, 533.0, 951.0, 90.0; waypoints.push_back(waypoint); - Eigen::VectorXd maxVelocities(4); - maxVelocities << 1.3, 0.67, 0.67, 0.5; - Eigen::VectorXd maxAccelerations(4); - maxAccelerations << 0.002, 0.002, 0.002, 0.002; + Eigen::VectorXd max_velocities(4); + max_velocities << 1.3, 0.67, 0.67, 0.5; + Eigen::VectorXd max_accelerations(4); + max_accelerations << 0.002, 0.002, 0.002, 0.002; - Trajectory trajectory(Path(waypoints, 100.0), maxVelocities, maxAccelerations, 10.0); + Trajectory trajectory(Path(waypoints, 100.0), max_velocities, max_accelerations, 10.0); EXPECT_TRUE(trajectory.isValid()); EXPECT_DOUBLE_EQ(1922.1418427445944, trajectory.getDuration()); @@ -128,12 +128,12 @@ TEST(time_optimal_trajectory_generation, test3) waypoint << 452.5, 533.0, 951.0, 90.0; waypoints.push_back(waypoint); - Eigen::VectorXd maxVelocities(4); - maxVelocities << 1.3, 0.67, 0.67, 0.5; - Eigen::VectorXd maxAccelerations(4); - maxAccelerations << 0.002, 0.002, 0.002, 0.002; + Eigen::VectorXd max_velocities(4); + max_velocities << 1.3, 0.67, 0.67, 0.5; + Eigen::VectorXd max_accelerations(4); + max_accelerations << 0.002, 0.002, 0.002, 0.002; - Trajectory trajectory(Path(waypoints, 100.0), maxVelocities, maxAccelerations); + Trajectory trajectory(Path(waypoints, 100.0), max_velocities, max_accelerations); EXPECT_TRUE(trajectory.isValid()); EXPECT_DOUBLE_EQ(1919.5597888812974, trajectory.getDuration()); diff --git a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp index 6953ff0499..a27a834141 100644 --- a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp +++ b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp @@ -44,8 +44,8 @@ #include // Static variables used in all tests -moveit::core::RobotModelConstPtr rmodel = moveit::core::loadTestingRobotModel("pr2"); -robot_trajectory::RobotTrajectory trajectory(rmodel, "right_arm"); +moveit::core::RobotModelConstPtr RMODEL = moveit::core::loadTestingRobotModel("pr2"); +robot_trajectory::RobotTrajectory TRAJECTORY(RMODEL, "right_arm"); // Initialize one-joint, 3 points exactly the same. int initRepeatedPointTrajectory(robot_trajectory::RobotTrajectory& trajectory) @@ -136,48 +136,48 @@ void printTrajectory(robot_trajectory::RobotTrajectory& trajectory) TEST(TestTimeParameterization, TestIterativeParabolic) { trajectory_processing::IterativeParabolicTimeParameterization time_parameterization; - EXPECT_EQ(initStraightTrajectory(trajectory), 0); + EXPECT_EQ(initStraightTrajectory(TRAJECTORY), 0); ros::WallTime wt = ros::WallTime::now(); - EXPECT_TRUE(time_parameterization.computeTimeStamps(trajectory)); + EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY)); std::cout << "IterativeParabolicTimeParameterization took " << (ros::WallTime::now() - wt).toSec() << std::endl; - printTrajectory(trajectory); - ASSERT_LT(trajectory.getWayPointDurationFromStart(trajectory.getWayPointCount() - 1), 3.0); + printTrajectory(TRAJECTORY); + ASSERT_LT(TRAJECTORY.getWayPointDurationFromStart(TRAJECTORY.getWayPointCount() - 1), 3.0); } TEST(TestTimeParameterization, TestIterativeSpline) { trajectory_processing::IterativeSplineParameterization time_parameterization(false); - EXPECT_EQ(initStraightTrajectory(trajectory), 0); + EXPECT_EQ(initStraightTrajectory(TRAJECTORY), 0); ros::WallTime wt = ros::WallTime::now(); - EXPECT_TRUE(time_parameterization.computeTimeStamps(trajectory)); + EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY)); std::cout << "IterativeSplineParameterization took " << (ros::WallTime::now() - wt).toSec() << std::endl; - printTrajectory(trajectory); - ASSERT_LT(trajectory.getWayPointDurationFromStart(trajectory.getWayPointCount() - 1), 5.0); + printTrajectory(TRAJECTORY); + ASSERT_LT(TRAJECTORY.getWayPointDurationFromStart(TRAJECTORY.getWayPointCount() - 1), 5.0); } TEST(TestTimeParameterization, TestIterativeSplineAddPoints) { trajectory_processing::IterativeSplineParameterization time_parameterization(true); - EXPECT_EQ(initStraightTrajectory(trajectory), 0); + EXPECT_EQ(initStraightTrajectory(TRAJECTORY), 0); ros::WallTime wt = ros::WallTime::now(); - EXPECT_TRUE(time_parameterization.computeTimeStamps(trajectory)); + EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY)); std::cout << "IterativeSplineParameterization with added points took " << (ros::WallTime::now() - wt).toSec() << std::endl; - printTrajectory(trajectory); - ASSERT_LT(trajectory.getWayPointDurationFromStart(trajectory.getWayPointCount() - 1), 5.0); + printTrajectory(TRAJECTORY); + ASSERT_LT(TRAJECTORY.getWayPointDurationFromStart(TRAJECTORY.getWayPointCount() - 1), 5.0); } TEST(TestTimeParameterization, TestRepeatedPoint) { trajectory_processing::IterativeSplineParameterization time_parameterization(true); - EXPECT_EQ(initRepeatedPointTrajectory(trajectory), 0); + EXPECT_EQ(initRepeatedPointTrajectory(TRAJECTORY), 0); - EXPECT_TRUE(time_parameterization.computeTimeStamps(trajectory)); - printTrajectory(trajectory); - ASSERT_LT(trajectory.getWayPointDurationFromStart(trajectory.getWayPointCount() - 1), 0.001); + EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY)); + printTrajectory(TRAJECTORY); + ASSERT_LT(TRAJECTORY.getWayPointDurationFromStart(TRAJECTORY.getWayPointCount() - 1), 0.001); } int main(int argc, char** argv) diff --git a/moveit_kinematics/kdl_kinematics_plugin/src/chainiksolver_vel_mimic_svd.cpp b/moveit_kinematics/kdl_kinematics_plugin/src/chainiksolver_vel_mimic_svd.cpp index c9e78e9556..19797e35c0 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/src/chainiksolver_vel_mimic_svd.cpp +++ b/moveit_kinematics/kdl_kinematics_plugin/src/chainiksolver_vel_mimic_svd.cpp @@ -86,6 +86,7 @@ bool ChainIkSolverVelMimicSVD::jacToJacReduced(const Jacobian& jac, Jacobian& ja return true; } +// NOLINTNEXTLINE(readability-identifier-naming) int ChainIkSolverVelMimicSVD::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out, const Eigen::VectorXd& joint_weights, const Eigen::Matrix& cartesian_weights) @@ -101,10 +102,10 @@ int ChainIkSolverVelMimicSVD::CartToJnt(const JntArray& q_in, const Twist& v_in, jnt2jac_.JntToJac(q_in, jac_reduced_); // weight Jacobian - auto& J = jac_reduced_.data; + auto& jac = jac_reduced_.data; const Eigen::Index rows = svd_.rows(); // only operate on position rows? - J.topRows(rows) *= joint_weights.asDiagonal(); - J.topRows(rows).transpose() *= cartesian_weights.topRows(rows).asDiagonal(); + jac.topRows(rows) *= joint_weights.asDiagonal(); + jac.topRows(rows).transpose() *= cartesian_weights.topRows(rows).asDiagonal(); // transform v_in to 6D Eigen::Vector Eigen::Matrix vin; @@ -112,7 +113,7 @@ int ChainIkSolverVelMimicSVD::CartToJnt(const JntArray& q_in, const Twist& v_in, vin.bottomRows<3>() = Eigen::Map(v_in.rot.data, 3) * cartesian_weights.bottomRows<3>().array(); // Do a singular value decomposition: J = U*S*V^t - svd_.compute(J.topRows(rows)); + svd_.compute(jac.topRows(rows)); if (num_mimic_joints_ > 0) { diff --git a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp index 0b26fbceb6..659556db78 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp +++ b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp @@ -429,6 +429,7 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c return false; } +// NOLINTNEXTLINE(readability-identifier-naming) int KDLKinematicsPlugin::CartToJnt(KDL::ChainIkSolverVelMimicSVD& ik_solver, const KDL::JntArray& q_init, const KDL::Frame& p_in, KDL::JntArray& q_out, const unsigned int max_iter, const Eigen::VectorXd& joint_weights, const Twist& cartesian_weights) const diff --git a/moveit_kinematics/test/test_kinematics_plugin.cpp b/moveit_kinematics/test/test_kinematics_plugin.cpp index b91b71cec4..31430c88f7 100644 --- a/moveit_kinematics/test/test_kinematics_plugin.cpp +++ b/moveit_kinematics/test/test_kinematics_plugin.cpp @@ -143,8 +143,8 @@ class SharedData static const SharedData& instance() { - static SharedData instance_; - return instance_; + static SharedData instance; + return instance; } static void release() { @@ -346,7 +346,7 @@ TEST_F(KinematicsTest, randomWalkIK) robot_trajectory::RobotTrajectory traj(robot_model_, jmg_); unsigned int failures = 0; - constexpr double NEAR_JOINT = 0.1; + static constexpr double NEAR_JOINT = 0.1; const std::vector consistency_limits(jmg_->getVariableCount(), 1.05 * NEAR_JOINT); for (unsigned int i = 0; i < num_ik_tests_; ++i) { @@ -480,7 +480,7 @@ TEST_F(KinematicsTest, unitIK) Eigen::Isometry3d initial, goal; tf2::fromMsg(poses[0], initial); - auto validateIK = [&](const geometry_msgs::Pose& goal, std::vector& truth) { + auto validate_ik = [&](const geometry_msgs::Pose& goal, std::vector& truth) { // compute IK moveit_msgs::MoveItErrorCodes error_code; kinematics_solver_->searchPositionIK(goal, seed, timeout_, @@ -528,7 +528,7 @@ TEST_F(KinematicsTest, unitIK) } { SCOPED_TRACE(desc); - validateIK(tf2::toMsg(goal), ground_truth); + validate_ik(tf2::toMsg(goal), ground_truth); } } } @@ -707,7 +707,7 @@ TEST_F(KinematicsTest, getNearestIKSolution) continue; const Eigen::Map seed_eigen(seed.data(), seed.size()); - double error_getIK = + double error_get_ik = (Eigen::Map(solution.data(), solution.size()) - seed_eigen).array().abs().sum(); // getPositionIK for multiple solutions @@ -720,15 +720,15 @@ TEST_F(KinematicsTest, getNearestIKSolution) if (result.kinematic_error != kinematics::KinematicErrors::OK) continue; - double smallest_error_multipleIK = std::numeric_limits::max(); + double smallest_error_multiple_ik = std::numeric_limits::max(); for (const auto& s : solutions) { - double error_multipleIK = + double error_multiple_ik = (Eigen::Map(s.data(), s.size()) - seed_eigen).array().abs().sum(); - if (error_multipleIK <= smallest_error_multipleIK) - smallest_error_multipleIK = error_multipleIK; + if (error_multiple_ik <= smallest_error_multiple_ik) + smallest_error_multiple_ik = error_multiple_ik; } - EXPECT_NEAR(smallest_error_multipleIK, error_getIK, tolerance_); + EXPECT_NEAR(smallest_error_multiple_ik, error_get_ik, tolerance_); } } diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp index b308e12022..e3bf05a69f 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp @@ -317,8 +317,8 @@ bool ChompOptimizer::optimize() ros::WallTime start_time = ros::WallTime::now(); // double averageCostVelocity = 0.0; // int currentCostIter = 0; - int costWindow = 10; - std::vector costs(costWindow, 0.0); + int cost_window = 10; + std::vector costs(cost_window, 0.0); // double minimaThreshold = 0.05; bool should_break_out = false; @@ -328,9 +328,9 @@ bool ChompOptimizer::optimize() ros::WallTime for_time = ros::WallTime::now(); performForwardKinematics(); ROS_INFO_STREAM("Forward kinematics took " << (ros::WallTime::now() - for_time)); - double cCost = getCollisionCost(); - double sCost = getSmoothnessCost(); - double cost = cCost + sCost; + double c_cost = getCollisionCost(); + double s_cost = getSmoothnessCost(); + double cost = c_cost + s_cost; // ROS_INFO_STREAM("Collision cost " << cCost << " smoothness cost " << sCost); @@ -432,7 +432,7 @@ bool ChompOptimizer::optimize() if (!parameters_->filter_mode_) { - if (cCost < parameters_->collision_threshold_) + if (c_cost < parameters_->collision_threshold_) { num_collision_free_iterations_ = parameters_->max_iterations_after_collision_free_; is_collision_free_ = true; @@ -615,26 +615,26 @@ void ChompOptimizer::calculateCollisionIncrements() collision_increments_.setZero(num_vars_free_, num_joints_); - int startPoint = 0; - int endPoint = free_vars_end_; + int start_point = 0; + int end_point = free_vars_end_; // In stochastic descent, simply use a random point in the trajectory, rather than all the trajectory points. // This is faster and guaranteed to converge, but it may take more iterations in the worst case. if (parameters_->use_stochastic_descent_) { - startPoint = (int)(((double)random() / (double)RAND_MAX) * (free_vars_end_ - free_vars_start_) + free_vars_start_); - if (startPoint < free_vars_start_) - startPoint = free_vars_start_; - if (startPoint > free_vars_end_) - startPoint = free_vars_end_; - endPoint = startPoint; + start_point = (int)(((double)random() / (double)RAND_MAX) * (free_vars_end_ - free_vars_start_) + free_vars_start_); + if (start_point < free_vars_start_) + start_point = free_vars_start_; + if (start_point > free_vars_end_) + start_point = free_vars_end_; + end_point = start_point; } else { - startPoint = free_vars_start_; + start_point = free_vars_start_; } - for (int i = startPoint; i <= endPoint; i++) + for (int i = start_point; i <= end_point; i++) { for (int j = 0; j < num_collision_points_; j++) { diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp index 0fdef81d9b..131c12f133 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp @@ -208,12 +208,12 @@ void ChompTrajectory::fillInMinJerk() { double start_index = start_index_ - 1; double end_index = end_index_ + 1; - double T[6]; // powers of the time duration - T[0] = 1.0; - T[1] = (end_index - start_index) * discretization_; + double td[6]; // powers of the time duration + td[0] = 1.0; + td[1] = (end_index - start_index) * discretization_; for (int i = 2; i <= 5; i++) - T[i] = T[i - 1] * T[1]; + td[i] = td[i - 1] * td[1]; // calculate the spline coefficients for each joint: // (these are for the special case of zero start and end vel and acc) @@ -225,26 +225,26 @@ void ChompTrajectory::fillInMinJerk() coeff[i][0] = x0; coeff[i][1] = 0; coeff[i][2] = 0; - coeff[i][3] = (-20 * x0 + 20 * x1) / (2 * T[3]); - coeff[i][4] = (30 * x0 - 30 * x1) / (2 * T[4]); - coeff[i][5] = (-12 * x0 + 12 * x1) / (2 * T[5]); + coeff[i][3] = (-20 * x0 + 20 * x1) / (2 * td[3]); + coeff[i][4] = (30 * x0 - 30 * x1) / (2 * td[4]); + coeff[i][5] = (-12 * x0 + 12 * x1) / (2 * td[5]); } // now fill in the joint positions at each time step for (int i = start_index + 1; i < end_index; i++) { - double t[6]; // powers of the time index point - t[0] = 1.0; - t[1] = (i - start_index) * discretization_; + double ti[6]; // powers of the time index point + ti[0] = 1.0; + ti[1] = (i - start_index) * discretization_; for (int k = 2; k <= 5; k++) - t[k] = t[k - 1] * t[1]; + ti[k] = ti[k - 1] * ti[1]; for (int j = 0; j < num_joints_; j++) { (*this)(i, j) = 0.0; for (int k = 0; k <= 5; k++) { - (*this)(i, j) += t[k] * coeff[j][k]; + (*this)(i, j) += ti[k] * coeff[j][k]; } } } diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp b/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp index ff20ded6a6..b9cfbcf5d6 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp +++ b/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp @@ -178,7 +178,7 @@ class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter ROS_INFO_STREAM("Configuring Planning Scene for CHOMP ...."); planning_scene->setActiveCollisionDetector(hybrid_cd, true); - chomp::ChompPlanner chompPlanner; + chomp::ChompPlanner chomp_planner; planning_interface::MotionPlanDetailedResponse res_detailed; moveit_msgs::MotionPlanDetailedResponse res_detailed_moveit_msgs; @@ -190,7 +190,7 @@ class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter res_detailed_moveit_msgs.trajectory.resize(1); res_detailed_moveit_msgs.trajectory[0] = trajectory_msgs_from_response; - bool planning_success = chompPlanner.solve(planning_scene, req, params_, res_detailed_moveit_msgs); + bool planning_success = chomp_planner.solve(planning_scene, req, params_, res_detailed_moveit_msgs); if (planning_success) { diff --git a/moveit_planners/ompl/ompl_interface/src/constraints_library.cpp b/moveit_planners/ompl/ompl_interface/src/constraints_library.cpp index 207af98365..1d962018ce 100644 --- a/moveit_planners/ompl/ompl_interface/src/constraints_library.cpp +++ b/moveit_planners/ompl/ompl_interface/src/constraints_library.cpp @@ -50,7 +50,7 @@ namespace template void msgToHex(const T& msg, std::string& hex) { - static const char symbol[] = { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' }; + static const char SYMBOL[] = { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' }; const size_t serial_size_arg = ros::serialization::serializationLength(msg); boost::shared_array buffer_arg(new uint8_t[serial_size_arg]); @@ -59,8 +59,8 @@ void msgToHex(const T& msg, std::string& hex) hex.resize(serial_size_arg * 2); for (std::size_t i = 0; i < serial_size_arg; ++i) { - hex[i * 2] = symbol[buffer_arg[i] / 16]; - hex[i * 2 + 1] = symbol[buffer_arg[i] % 16]; + hex[i * 2] = SYMBOL[buffer_arg[i] / 16]; + hex[i * 2 + 1] = SYMBOL[buffer_arg[i] % 16]; } } diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index a1e3bcaa20..88a0666871 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -332,8 +332,8 @@ const ompl_interface::ModelBasedStateSpaceFactoryPtr& ompl_interface::PlanningCo else { ROS_ERROR_NAMED("planning_context_manager", "Factory of type '%s' was not found", factory_type.c_str()); - static const ModelBasedStateSpaceFactoryPtr empty; - return empty; + static const ModelBasedStateSpaceFactoryPtr EMPTY; + return EMPTY; } } @@ -358,8 +358,8 @@ const ompl_interface::ModelBasedStateSpaceFactoryPtr& ompl_interface::PlanningCo { ROS_ERROR_NAMED("planning_context_manager", "There are no known state spaces that can represent the given planning " "problem"); - static const ModelBasedStateSpaceFactoryPtr empty; - return empty; + static const ModelBasedStateSpaceFactoryPtr EMPTY; + return EMPTY; } else { diff --git a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.cpp b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.cpp index 9cc5aec2d3..156142157e 100644 --- a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.cpp +++ b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.cpp @@ -165,7 +165,7 @@ void ViaPointController::execTrajectory(const moveit_msgs::RobotTrajectory& t) // publish joint states for all intermediate via points of the trajectory // no further interpolation - ros::Time startTime = ros::Time::now(); + ros::Time start_time = ros::Time::now(); for (std::vector::const_iterator via = t.joint_trajectory.points.begin(), end = t.joint_trajectory.points.end(); !cancelled() && via != end; ++via) @@ -174,11 +174,11 @@ void ViaPointController::execTrajectory(const moveit_msgs::RobotTrajectory& t) js.velocity = via->velocities; js.effort = via->effort; - ros::Duration waitTime = via->time_from_start - (ros::Time::now() - startTime); - if (waitTime.toSec() > std::numeric_limits::epsilon()) + ros::Duration wait_time = via->time_from_start - (ros::Time::now() - start_time); + if (wait_time.toSec() > std::numeric_limits::epsilon()) { - ROS_DEBUG("Fake execution: waiting %0.1fs for next via point, %ld remaining", waitTime.toSec(), end - via); - waitTime.sleep(); + ROS_DEBUG("Fake execution: waiting %0.1fs for next via point, %ld remaining", wait_time.toSec(), end - via); + wait_time.sleep(); } js.header.stamp = ros::Time::now(); pub_.publish(js); @@ -230,10 +230,10 @@ void InterpolatingController::execTrajectory(const moveit_msgs::RobotTrajectory& next = points.begin() + 1, // currently targetted via point end = points.end(); - ros::Time startTime = ros::Time::now(); + ros::Time start_time = ros::Time::now(); while (!cancelled()) { - ros::Duration elapsed = ros::Time::now() - startTime; + ros::Duration elapsed = ros::Time::now() - start_time; // hop to next targetted via point while (next != end && elapsed > next->time_from_start) { @@ -256,7 +256,7 @@ void InterpolatingController::execTrajectory(const moveit_msgs::RobotTrajectory& if (cancelled()) return; - ros::Duration elapsed = ros::Time::now() - startTime; + ros::Duration elapsed = ros::Time::now() - start_time; ROS_DEBUG("elapsed: %.3f via points %td,%td / %td alpha: 1.0", elapsed.toSec(), prev - points.begin(), next - points.begin(), end - points.begin()); diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index 9be3889377..92b38ae528 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -826,7 +826,7 @@ void BenchmarkExecutor::collectMetrics(PlannerRunData& metrics, if (solved) { // Analyzing the trajectory(ies) geometrically - double L = 0.0; // trajectory length + double traj_len = 0.0; // trajectory length double clearance = 0.0; // trajectory clearance (average) double smoothness = 0.0; // trajectory smoothness (average) bool correct = true; // entire trajectory collision free and in bounds @@ -835,14 +835,14 @@ void BenchmarkExecutor::collectMetrics(PlannerRunData& metrics, for (std::size_t j = 0; j < mp_res.trajectory_.size(); ++j) { correct = true; - L = 0.0; + traj_len = 0.0; clearance = 0.0; smoothness = 0.0; const robot_trajectory::RobotTrajectory& p = *mp_res.trajectory_[j]; // compute path length for (std::size_t k = 1; k < p.getWayPointCount(); ++k) - L += p.getWayPoint(k - 1).distance(p.getWayPoint(k)); + traj_len += p.getWayPoint(k - 1).distance(p.getWayPoint(k)); // compute correctness and clearance collision_detection::CollisionRequest req; @@ -878,11 +878,11 @@ void BenchmarkExecutor::collectMetrics(PlannerRunData& metrics, // use Pythagoras generalized theorem to find the cos of the angle between segments a and b double b = p.getWayPoint(k - 1).distance(p.getWayPoint(k)); double cdist = p.getWayPoint(k - 2).distance(p.getWayPoint(k)); - double acosValue = (a * a + b * b - cdist * cdist) / (2.0 * a * b); - if (acosValue > -1.0 && acosValue < 1.0) + double acos_value = (a * a + b * b - cdist * cdist) / (2.0 * a * b); + if (acos_value > -1.0 && acos_value < 1.0) { // the smoothness is actually the outside angle of the one we compute - double angle = (boost::math::constants::pi() - acos(acosValue)); + double angle = (boost::math::constants::pi() - acos(acos_value)); // and we normalize by the length of the segments double u = 2.0 * angle; /// (a + b); @@ -893,7 +893,7 @@ void BenchmarkExecutor::collectMetrics(PlannerRunData& metrics, smoothness /= (double)p.getWayPointCount(); } metrics["path_" + mp_res.description_[j] + "_correct BOOLEAN"] = boost::lexical_cast(correct); - metrics["path_" + mp_res.description_[j] + "_length REAL"] = moveit::core::toString(L); + metrics["path_" + mp_res.description_[j] + "_length REAL"] = moveit::core::toString(traj_len); metrics["path_" + mp_res.description_[j] + "_clearance REAL"] = moveit::core::toString(clearance); metrics["path_" + mp_res.description_[j] + "_smoothness REAL"] = moveit::core::toString(smoothness); metrics["path_" + mp_res.description_[j] + "_time REAL"] = moveit::core::toString(mp_res.processing_time_[j]); diff --git a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp index fd23cde7b8..4d413e1c9b 100644 --- a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp +++ b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp @@ -86,8 +86,8 @@ void move_group::MoveGroupPickPlaceAction::startPlaceLookCallback() setPlaceState(LOOK); } -void move_group::MoveGroupPickPlaceAction::executePickupCallback_PlanOnly(const moveit_msgs::PickupGoalConstPtr& goal, - moveit_msgs::PickupResult& action_res) +void move_group::MoveGroupPickPlaceAction::executePickupCallbackPlanOnly(const moveit_msgs::PickupGoalConstPtr& goal, + moveit_msgs::PickupResult& action_res) { pick_place::PickPlanPtr plan; try @@ -126,8 +126,8 @@ void move_group::MoveGroupPickPlaceAction::executePickupCallback_PlanOnly(const } } -void move_group::MoveGroupPickPlaceAction::executePlaceCallback_PlanOnly(const moveit_msgs::PlaceGoalConstPtr& goal, - moveit_msgs::PlaceResult& action_res) +void move_group::MoveGroupPickPlaceAction::executePlaceCallbackPlanOnly(const moveit_msgs::PlaceGoalConstPtr& goal, + moveit_msgs::PlaceResult& action_res) { pick_place::PlacePlanPtr plan; try @@ -166,9 +166,9 @@ void move_group::MoveGroupPickPlaceAction::executePlaceCallback_PlanOnly(const m } } -bool move_group::MoveGroupPickPlaceAction::planUsingPickPlace_Pickup(const moveit_msgs::PickupGoal& goal, - moveit_msgs::PickupResult* action_res, - plan_execution::ExecutableMotionPlan& plan) +bool move_group::MoveGroupPickPlaceAction::planUsingPickPlacePickup(const moveit_msgs::PickupGoal& goal, + moveit_msgs::PickupResult* action_res, + plan_execution::ExecutableMotionPlan& plan) { setPickupState(PLANNING); @@ -209,9 +209,9 @@ bool move_group::MoveGroupPickPlaceAction::planUsingPickPlace_Pickup(const movei return plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS; } -bool move_group::MoveGroupPickPlaceAction::planUsingPickPlace_Place(const moveit_msgs::PlaceGoal& goal, - moveit_msgs::PlaceResult* action_res, - plan_execution::ExecutableMotionPlan& plan) +bool move_group::MoveGroupPickPlaceAction::planUsingPickPlacePlace(const moveit_msgs::PlaceGoal& goal, + moveit_msgs::PlaceResult* action_res, + plan_execution::ExecutableMotionPlan& plan) { setPlaceState(PLANNING); @@ -252,7 +252,7 @@ bool move_group::MoveGroupPickPlaceAction::planUsingPickPlace_Place(const moveit return plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS; } -void move_group::MoveGroupPickPlaceAction::executePickupCallback_PlanAndExecute( +void move_group::MoveGroupPickPlaceAction::executePickupCallbackPlanAndExecute( const moveit_msgs::PickupGoalConstPtr& goal, moveit_msgs::PickupResult& action_res) { plan_execution::PlanExecution::Options opt; @@ -263,7 +263,7 @@ void move_group::MoveGroupPickPlaceAction::executePickupCallback_PlanAndExecute( opt.before_execution_callback_ = boost::bind(&MoveGroupPickPlaceAction::startPickupExecutionCallback, this); opt.plan_callback_ = - boost::bind(&MoveGroupPickPlaceAction::planUsingPickPlace_Pickup, this, boost::cref(*goal), &action_res, _1); + boost::bind(&MoveGroupPickPlaceAction::planUsingPickPlacePickup, this, boost::cref(*goal), &action_res, _1); if (goal->planning_options.look_around && context_->plan_with_sensing_) { opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(), @@ -283,7 +283,7 @@ void move_group::MoveGroupPickPlaceAction::executePickupCallback_PlanAndExecute( action_res.error_code = plan.error_code_; } -void move_group::MoveGroupPickPlaceAction::executePlaceCallback_PlanAndExecute( +void move_group::MoveGroupPickPlaceAction::executePlaceCallbackPlanAndExecute( const moveit_msgs::PlaceGoalConstPtr& goal, moveit_msgs::PlaceResult& action_res) { plan_execution::PlanExecution::Options opt; @@ -293,7 +293,7 @@ void move_group::MoveGroupPickPlaceAction::executePlaceCallback_PlanAndExecute( opt.replan_delay_ = goal->planning_options.replan_delay; opt.before_execution_callback_ = boost::bind(&MoveGroupPickPlaceAction::startPlaceExecutionCallback, this); opt.plan_callback_ = - boost::bind(&MoveGroupPickPlaceAction::planUsingPickPlace_Place, this, boost::cref(*goal), &action_res, _1); + boost::bind(&MoveGroupPickPlaceAction::planUsingPickPlacePlace, this, boost::cref(*goal), &action_res, _1); if (goal->planning_options.look_around && context_->plan_with_sensing_) { opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(), @@ -339,10 +339,10 @@ void move_group::MoveGroupPickPlaceAction::executePickupCallback(const moveit_ms ROS_WARN_NAMED("manipulation", "This instance of MoveGroup is not allowed to execute trajectories but the pick " "goal request has plan_only set to false. Only a motion plan will be computed " "anyway."); - executePickupCallback_PlanOnly(goal, action_res); + executePickupCallbackPlanOnly(goal, action_res); } else - executePickupCallback_PlanAndExecute(goal, action_res); + executePickupCallbackPlanAndExecute(goal, action_res); bool planned_trajectory_empty = action_res.trajectory_stages.empty(); std::string response = @@ -376,10 +376,10 @@ void move_group::MoveGroupPickPlaceAction::executePlaceCallback(const moveit_msg ROS_WARN_NAMED("manipulation", "This instance of MoveGroup is not allowed to execute trajectories but the place " "goal request has plan_only set to false. Only a motion plan will be computed " "anyway."); - executePlaceCallback_PlanOnly(goal, action_res); + executePlaceCallbackPlanOnly(goal, action_res); } else - executePlaceCallback_PlanAndExecute(goal, action_res); + executePlaceCallbackPlanAndExecute(goal, action_res); bool planned_trajectory_empty = action_res.trajectory_stages.empty(); std::string response = diff --git a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h index 284812490b..d76e6244f2 100644 --- a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h +++ b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h @@ -57,19 +57,19 @@ class MoveGroupPickPlaceAction : public MoveGroupCapability void executePickupCallback(const moveit_msgs::PickupGoalConstPtr& goal); void executePlaceCallback(const moveit_msgs::PlaceGoalConstPtr& goal); - void executePickupCallback_PlanOnly(const moveit_msgs::PickupGoalConstPtr& goal, - moveit_msgs::PickupResult& action_res); - void executePickupCallback_PlanAndExecute(const moveit_msgs::PickupGoalConstPtr& goal, - moveit_msgs::PickupResult& action_res); - - void executePlaceCallback_PlanOnly(const moveit_msgs::PlaceGoalConstPtr& goal, moveit_msgs::PlaceResult& action_res); - void executePlaceCallback_PlanAndExecute(const moveit_msgs::PlaceGoalConstPtr& goal, - moveit_msgs::PlaceResult& action_res); - - bool planUsingPickPlace_Pickup(const moveit_msgs::PickupGoal& goal, moveit_msgs::PickupResult* action_res, - plan_execution::ExecutableMotionPlan& plan); - bool planUsingPickPlace_Place(const moveit_msgs::PlaceGoal& goal, moveit_msgs::PlaceResult* action_res, + void executePickupCallbackPlanOnly(const moveit_msgs::PickupGoalConstPtr& goal, + moveit_msgs::PickupResult& action_res); + void executePickupCallbackPlanAndExecute(const moveit_msgs::PickupGoalConstPtr& goal, + moveit_msgs::PickupResult& action_res); + + void executePlaceCallbackPlanOnly(const moveit_msgs::PlaceGoalConstPtr& goal, moveit_msgs::PlaceResult& action_res); + void executePlaceCallbackPlanAndExecute(const moveit_msgs::PlaceGoalConstPtr& goal, + moveit_msgs::PlaceResult& action_res); + + bool planUsingPickPlacePickup(const moveit_msgs::PickupGoal& goal, moveit_msgs::PickupResult* action_res, plan_execution::ExecutableMotionPlan& plan); + bool planUsingPickPlacePlace(const moveit_msgs::PlaceGoal& goal, moveit_msgs::PlaceResult* action_res, + plan_execution::ExecutableMotionPlan& plan); void preemptPickupCallback(); void preemptPlaceCallback(); diff --git a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp index 2c69367f7f..7ba01ea556 100644 --- a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp +++ b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp @@ -221,7 +221,7 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const planning_scene_->getFrameTransform(plan->retreat_.direction.header.frame_id).rotation() * retreat_direction; // state validity checking during the approach must ensure that the gripper posture is that for pre-grasping - robot_state::GroupStateValidityCallbackFn approach_validCallback = + robot_state::GroupStateValidityCallbackFn approach_valid_callback = boost::bind(&isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_, &plan->approach_posture_, _1, _2, _3); plan->goal_sampler_->setVerbose(verbose_); @@ -239,7 +239,7 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const std::vector close_up_states; double d_close_up = close_up_state->computeCartesianPath( plan->shared_data_->planning_group_, close_up_states, plan->shared_data_->ik_link_, approach_direction, - approach_direction_is_global_frame, MAX_CLOSE_UP_DIST, max_step_, jump_factor_, approach_validCallback); + approach_direction_is_global_frame, MAX_CLOSE_UP_DIST, max_step_, jump_factor_, approach_valid_callback); // if progress towards the object was made, update the desired goal state if (d_close_up > 0.0 && close_up_states.size() > 1) *plan->possible_goal_states_[i] = *close_up_states[close_up_states.size() - 2]; @@ -252,7 +252,7 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const double d_approach = first_approach_state->computeCartesianPath( plan->shared_data_->planning_group_, approach_states, plan->shared_data_->ik_link_, -approach_direction, approach_direction_is_global_frame, plan->approach_.desired_distance, max_step_, jump_factor_, - approach_validCallback); + approach_valid_callback); // if we were able to follow the approach direction for sufficient length, try to compute a retreat direction if (d_approach > plan->approach_.min_distance && !signal_stop_) @@ -270,7 +270,7 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const // state validity checking during the retreat after the grasp must ensure the gripper posture is that of the // actual grasp - robot_state::GroupStateValidityCallbackFn retreat_validCallback = + robot_state::GroupStateValidityCallbackFn retreat_valid_callback = boost::bind(&isStateCollisionFree, planning_scene_after_approach.get(), collision_matrix_.get(), verbose_, &plan->retreat_posture_, _1, _2, _3); @@ -281,7 +281,7 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const double d_retreat = last_retreat_state->computeCartesianPath( plan->shared_data_->planning_group_, retreat_states, plan->shared_data_->ik_link_, retreat_direction, retreat_direction_is_global_frame, plan->retreat_.desired_distance, max_step_, jump_factor_, - retreat_validCallback); + retreat_valid_callback); // if sufficient progress was made in the desired direction, we have a goal state that we can consider for // future stages diff --git a/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp b/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp index 75403f33c6..e75a9074dc 100644 --- a/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp +++ b/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp @@ -61,8 +61,8 @@ const ManipulationStagePtr& ManipulationPipeline::getFirstStage() const { if (stages_.empty()) { - static const ManipulationStagePtr empty; - return empty; + static const ManipulationStagePtr EMPTY; + return EMPTY; } else return stages_.front(); @@ -72,8 +72,8 @@ const ManipulationStagePtr& ManipulationPipeline::getLastStage() const { if (stages_.empty()) { - static const ManipulationStagePtr empty; - return empty; + static const ManipulationStagePtr EMPTY; + return EMPTY; } else return stages_.back(); diff --git a/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp b/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp index 644cfa41a6..250c42b621 100644 --- a/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp +++ b/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp @@ -80,6 +80,6 @@ pick_place::PickPlaceParams::PickPlaceParams() : max_goal_count_(5), max_fail_(3 const pick_place::PickPlaceParams& pick_place::GetGlobalPickPlaceParams() { - static DynamicReconfigureImpl PICK_PLACE_PARAMS; - return PICK_PLACE_PARAMS.getParams(); + static DynamicReconfigureImpl pick_place_params; + return pick_place_params.getParams(); } diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp index a68c7da143..17cf78eb2a 100644 --- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp @@ -70,10 +70,10 @@ void move_group::MoveGroupMoveAction::executeMoveCallback(const moveit_msgs::Mov if (!goal->planning_options.plan_only) ROS_WARN("This instance of MoveGroup is not allowed to execute trajectories but the goal request has plan_only " "set to false. Only a motion plan will be computed anyway."); - executeMoveCallback_PlanOnly(goal, action_res); + executeMoveCallbackPlanOnly(goal, action_res); } else - executeMoveCallback_PlanAndExecute(goal, action_res); + executeMoveCallbackPlanAndExecute(goal, action_res); bool planned_trajectory_empty = trajectory_processing::isTrajectoryEmpty(action_res.planned_trajectory); std::string response = @@ -93,8 +93,8 @@ void move_group::MoveGroupMoveAction::executeMoveCallback(const moveit_msgs::Mov preempt_requested_ = false; } -void move_group::MoveGroupMoveAction::executeMoveCallback_PlanAndExecute(const moveit_msgs::MoveGroupGoalConstPtr& goal, - moveit_msgs::MoveGroupResult& action_res) +void move_group::MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const moveit_msgs::MoveGroupGoalConstPtr& goal, + moveit_msgs::MoveGroupResult& action_res) { ROS_INFO("Combined planning and execution request received for MoveGroup action. Forwarding to planning and " "execution pipeline."); @@ -157,8 +157,8 @@ void move_group::MoveGroupMoveAction::executeMoveCallback_PlanAndExecute(const m action_res.error_code = plan.error_code_; } -void move_group::MoveGroupMoveAction::executeMoveCallback_PlanOnly(const moveit_msgs::MoveGroupGoalConstPtr& goal, - moveit_msgs::MoveGroupResult& action_res) +void move_group::MoveGroupMoveAction::executeMoveCallbackPlanOnly(const moveit_msgs::MoveGroupGoalConstPtr& goal, + moveit_msgs::MoveGroupResult& action_res) { ROS_INFO("Planning request received for MoveGroup action. Forwarding to planning pipeline."); diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.h b/moveit_ros/move_group/src/default_capabilities/move_action_capability.h index edd9b1813d..d8af647a38 100644 --- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/move_action_capability.h @@ -53,10 +53,10 @@ class MoveGroupMoveAction : public MoveGroupCapability private: void executeMoveCallback(const moveit_msgs::MoveGroupGoalConstPtr& goal); - void executeMoveCallback_PlanAndExecute(const moveit_msgs::MoveGroupGoalConstPtr& goal, - moveit_msgs::MoveGroupResult& action_res); - void executeMoveCallback_PlanOnly(const moveit_msgs::MoveGroupGoalConstPtr& goal, - moveit_msgs::MoveGroupResult& action_res); + void executeMoveCallbackPlanAndExecute(const moveit_msgs::MoveGroupGoalConstPtr& goal, + moveit_msgs::MoveGroupResult& action_res); + void executeMoveCallbackPlanOnly(const moveit_msgs::MoveGroupGoalConstPtr& goal, + moveit_msgs::MoveGroupResult& action_res); void startMoveExecutionCallback(); void startMoveLookCallback(); void preemptMoveCallback(); diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp index caa9e82177..ab239b69e4 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp +++ b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp @@ -120,7 +120,7 @@ bool DepthImageOctomapUpdater::initialize() // create our mesh filter mesh_filter_.reset(new mesh_filter::MeshFilter( - mesh_filter::MeshFilterBase::TransformCallback(), mesh_filter::StereoCameraModel::RegisteredPSDKParams)); + mesh_filter::MeshFilterBase::TransformCallback(), mesh_filter::StereoCameraModel::REGISTERED_PSDK_PARAMS)); mesh_filter_->parameters().setDepthRange(near_clipping_plane_distance_, far_clipping_plane_distance_); mesh_filter_->setShadowThreshold(shadow_threshold_); mesh_filter_->setPaddingOffset(padding_offset_); @@ -247,9 +247,9 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP monitor_->setMapFrame(depth_msg->header.frame_id); /* get transform for cloud into map frame */ - tf2::Stamped map_H_sensor; + tf2::Stamped map_h_sensor; if (monitor_->getMapFrame() == depth_msg->header.frame_id) - map_H_sensor.setIdentity(); + map_h_sensor.setIdentity(); else { if (tf_buffer_) @@ -264,15 +264,15 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP { tf2::fromMsg( tf_buffer_->lookupTransform(monitor_->getMapFrame(), depth_msg->header.frame_id, depth_msg->header.stamp), - map_H_sensor); + map_h_sensor); found = true; break; } catch (tf2::TransformException& ex) { - static const ros::Duration d(TEST_DT); + static const ros::Duration D(TEST_DT); err = ex.what(); - d.sleep(); + D.sleep(); } static const unsigned int MAX_TF_COUNTER = 1000; // so we avoid int overflow if (found) @@ -372,8 +372,8 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP y_cache_[y] = (y - py) * inv_fy_; } - const octomap::point3d sensor_origin(map_H_sensor.getOrigin().getX(), map_H_sensor.getOrigin().getY(), - map_H_sensor.getOrigin().getZ()); + const octomap::point3d sensor_origin(map_h_sensor.getOrigin().getX(), map_h_sensor.getOrigin().getY(), + map_h_sensor.getOrigin().getZ()); octomap::KeySet* occupied_cells_ptr = new octomap::KeySet(); octomap::KeySet* model_cells_ptr = new octomap::KeySet(); @@ -472,7 +472,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP float yy = y_cache_[y] * zz; float xx = x_cache_[x] * zz; /* transform to map frame */ - tf2::Vector3 point_tf = map_H_sensor * tf2::Vector3(xx, yy, zz); + tf2::Vector3 point_tf = map_h_sensor * tf2::Vector3(xx, yy, zz); occupied_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ())); } // on far plane or a model point -> remove @@ -482,7 +482,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP float yy = y_cache_[y] * zz; float xx = x_cache_[x] * zz; /* transform to map frame */ - tf2::Vector3 point_tf = map_H_sensor * tf2::Vector3(xx, yy, zz); + tf2::Vector3 point_tf = map_h_sensor * tf2::Vector3(xx, yy, zz); // add to the list of model cells model_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ())); } @@ -501,7 +501,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP float yy = y_cache_[y] * zz; float xx = x_cache_[x] * zz; /* transform to map frame */ - tf2::Vector3 point_tf = map_H_sensor * tf2::Vector3(xx, yy, zz); + tf2::Vector3 point_tf = map_h_sensor * tf2::Vector3(xx, yy, zz); occupied_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ())); } else if (labels_row[x] >= mesh_filter::MeshFilterBase::FarClip) @@ -510,7 +510,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP float yy = y_cache_[y] * zz; float xx = x_cache_[x] * zz; /* transform to map frame */ - tf2::Vector3 point_tf = map_H_sensor * tf2::Vector3(xx, yy, zz); + tf2::Vector3 point_tf = map_h_sensor * tf2::Vector3(xx, yy, zz); // add to the list of model cells model_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ())); } diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.h index b254f42e6a..e488aa2883 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.h @@ -93,9 +93,9 @@ class MeshFilter : public MeshFilterBase template MeshFilter::MeshFilter(const TransformCallback& transform_callback, const typename SensorType::Parameters& sensor_parameters) - : MeshFilterBase(transform_callback, sensor_parameters, SensorType::renderVertexShaderSource, - SensorType::renderFragmentShaderSource, SensorType::filterVertexShaderSource, - SensorType::filterFragmentShaderSource) + : MeshFilterBase(transform_callback, sensor_parameters, SensorType::RENDER_VERTEX_SHADER_SOURCE, + SensorType::RENDER_FRAGMENT_SHADER_SOURCE, SensorType::FILTER_VERTEX_SHADER_SOURCE, + SensorType::FILTER_FRAGMENT_SHADER_SOURCE) { } diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.h index 96ca3acbf6..2dfac68ad6 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.h @@ -149,19 +149,19 @@ class StereoCameraModel : public SensorModel }; /** \brief predefined sensor model for OpenNI compatible devices (e.g., PrimeSense, Kinect, Asus Xtion) */ - static const StereoCameraModel::Parameters& RegisteredPSDKParams; + static const StereoCameraModel::Parameters& REGISTERED_PSDK_PARAMS; /** \brief source code of the vertex shader used to render the meshes*/ - static const std::string renderVertexShaderSource; + static const std::string RENDER_VERTEX_SHADER_SOURCE; /** \brief source code of the fragment shader used to render the meshes*/ - static const std::string renderFragmentShaderSource; + static const std::string RENDER_FRAGMENT_SHADER_SOURCE; /** \brief source code of the vertex shader used to filter the depth map*/ - static const std::string filterVertexShaderSource; + static const std::string FILTER_VERTEX_SHADER_SOURCE; /** \brief source code of the fragment shader used to filter the depth map*/ - static const std::string filterFragmentShaderSource; + static const std::string FILTER_FRAGMENT_SHADER_SOURCE; }; } // namespace mesh_filter #endif diff --git a/moveit_ros/perception/mesh_filter/src/gl_mesh.cpp b/moveit_ros/perception/mesh_filter/src/gl_mesh.cpp index 0b0d6a4715..e7a08132de 100644 --- a/moveit_ros/perception/mesh_filter/src/gl_mesh.cpp +++ b/moveit_ros/perception/mesh_filter/src/gl_mesh.cpp @@ -54,11 +54,11 @@ mesh_filter::GLMesh::GLMesh(const Mesh& mesh, unsigned int mesh_label) glNewList(list_, GL_COMPILE); glBegin(GL_TRIANGLES); glColor4ubv((GLubyte*)&mesh_label_); - for (unsigned tIdx = 0; tIdx < mesh.triangle_count; ++tIdx) + for (unsigned t_idx = 0; t_idx < mesh.triangle_count; ++t_idx) { - unsigned v1 = 3 * mesh.triangles[3 * tIdx]; - unsigned v2 = 3 * mesh.triangles[3 * tIdx + 1]; - unsigned v3 = 3 * mesh.triangles[3 * tIdx + 2]; + unsigned v1 = 3 * mesh.triangles[3 * t_idx]; + unsigned v2 = 3 * mesh.triangles[3 * t_idx + 1]; + unsigned v3 = 3 * mesh.triangles[3 * t_idx + 2]; glNormal3f(mesh.vertex_normals[v1], mesh.vertex_normals[v1 + 1], mesh.vertex_normals[v1 + 2]); glVertex3f(mesh.vertices[v1], mesh.vertices[v1 + 1], mesh.vertices[v1 + 2]); diff --git a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp index 1e70399c96..bb0d9b7b18 100644 --- a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp +++ b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp @@ -157,8 +157,8 @@ void mesh_filter::GLRenderer::initFrameBuffers() glFramebufferTexture2D(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, depth_id_, 0); glBindRenderbuffer(GL_RENDERBUFFER, 0); - GLenum DrawBuffers[2] = { GL_COLOR_ATTACHMENT0, GL_DEPTH_ATTACHMENT }; - glDrawBuffers(2, DrawBuffers); + GLenum draw_buffers[2] = { GL_COLOR_ATTACHMENT0, GL_DEPTH_ATTACHMENT }; + glDrawBuffers(2, draw_buffers); GLenum status = glCheckFramebufferStatus(GL_FRAMEBUFFER); @@ -258,32 +258,32 @@ const float& mesh_filter::GLRenderer::getFarClippingDistance() const GLuint mesh_filter::GLRenderer::createShader(GLuint shaderType, const string& ShaderCode) const { - GLuint ShaderID = glCreateShader(shaderType); + GLuint shader_id = glCreateShader(shaderType); // Compile Shader - char const* SourcePointer = ShaderCode.c_str(); - glShaderSource(ShaderID, 1, &SourcePointer, nullptr); - glCompileShader(ShaderID); + char const* source_pointer = ShaderCode.c_str(); + glShaderSource(shader_id, 1, &source_pointer, nullptr); + glCompileShader(shader_id); // Check Shader - GLint Result = GL_FALSE; - glGetShaderiv(ShaderID, GL_COMPILE_STATUS, &Result); - if (Result != GL_TRUE) + GLint result = GL_FALSE; + glGetShaderiv(shader_id, GL_COMPILE_STATUS, &result); + if (result != GL_TRUE) { - int InfoLogLength; - glGetShaderiv(ShaderID, GL_INFO_LOG_LENGTH, &InfoLogLength); - if (InfoLogLength > 0) + int info_log_length; + glGetShaderiv(shader_id, GL_INFO_LOG_LENGTH, &info_log_length); + if (info_log_length > 0) { - vector ShaderErrorMessage(InfoLogLength + 1); - glGetShaderInfoLog(ShaderID, InfoLogLength, nullptr, &ShaderErrorMessage[0]); - stringstream errorStream; - errorStream << "Could not compile shader: " << (const char*)&ShaderErrorMessage[0]; + vector shader_error_message(info_log_length + 1); + glGetShaderInfoLog(shader_id, info_log_length, nullptr, &shader_error_message[0]); + stringstream error_stream; + error_stream << "Could not compile shader: " << (const char*)&shader_error_message[0]; - glDeleteShader(ShaderID); - throw runtime_error(errorStream.str()); + glDeleteShader(shader_id); + throw runtime_error(error_stream.str()); } } - return ShaderID; + return shader_id; } void mesh_filter::GLRenderer::readShaderCodeFromFile(const string& filename, string& shader) const @@ -292,19 +292,19 @@ void mesh_filter::GLRenderer::readShaderCodeFromFile(const string& filename, str shader = ""; else { - string ShaderCode; - fstream ShaderFile(filename.c_str(), ios::in); - if (ShaderFile.is_open()) + string shader_code; + fstream shader_file(filename.c_str(), ios::in); + if (shader_file.is_open()) { stringstream buffer; - buffer << ShaderFile.rdbuf(); + buffer << shader_file.rdbuf(); shader = buffer.str(); } else { - stringstream errorStream; - errorStream << "Could not open shader code in file \"" << filename << "\""; - throw runtime_error(errorStream.str()); + stringstream error_stream; + error_stream << "Could not open shader code in file \"" << filename << "\""; + throw runtime_error(error_stream.str()); } } } @@ -314,45 +314,45 @@ GLuint mesh_filter::GLRenderer::loadShaders(const string& vertex_source, const s if (vertex_source.empty() && fragment_source.empty()) return 0; - GLuint ProgramID = glCreateProgram(); - GLuint VertexShaderID = 0; - GLuint FragmentShaderID = 0; + GLuint program_id = glCreateProgram(); + GLuint vertex_shader_id = 0; + GLuint fragment_shader_id = 0; if (!vertex_source.empty()) { - GLuint VertexShaderID = createShader(GL_VERTEX_SHADER, vertex_source); - glAttachShader(ProgramID, VertexShaderID); + GLuint vertex_shader_id = createShader(GL_VERTEX_SHADER, vertex_source); + glAttachShader(program_id, vertex_shader_id); } if (!fragment_source.empty()) { - GLuint FragmentShaderID = createShader(GL_FRAGMENT_SHADER, fragment_source); - glAttachShader(ProgramID, FragmentShaderID); + GLuint fragment_shader_id = createShader(GL_FRAGMENT_SHADER, fragment_source); + glAttachShader(program_id, fragment_shader_id); } - glLinkProgram(ProgramID); + glLinkProgram(program_id); // Check the program - GLint Result = GL_FALSE; - GLint InfoLogLength; - glGetProgramiv(ProgramID, GL_LINK_STATUS, &Result); - glGetProgramiv(ProgramID, GL_INFO_LOG_LENGTH, &InfoLogLength); - if (InfoLogLength > 0) + GLint result = GL_FALSE; + GLint info_log_length; + glGetProgramiv(program_id, GL_LINK_STATUS, &result); + glGetProgramiv(program_id, GL_INFO_LOG_LENGTH, &info_log_length); + if (info_log_length > 0) { - vector ProgramErrorMessage(InfoLogLength + 1); - glGetProgramInfoLog(ProgramID, InfoLogLength, nullptr, &ProgramErrorMessage[0]); - std::size_t l = strnlen(&ProgramErrorMessage[0], ProgramErrorMessage.size()); + vector program_error_message(info_log_length + 1); + glGetProgramInfoLog(program_id, info_log_length, nullptr, &program_error_message[0]); + std::size_t l = strnlen(&program_error_message[0], program_error_message.size()); if (l > 0) - ROS_ERROR("%s\n", &ProgramErrorMessage[0]); + ROS_ERROR("%s\n", &program_error_message[0]); } - if (VertexShaderID) - glDeleteShader(VertexShaderID); + if (vertex_shader_id) + glDeleteShader(vertex_shader_id); - if (FragmentShaderID) - glDeleteShader(FragmentShaderID); + if (fragment_shader_id) + glDeleteShader(fragment_shader_id); - return ProgramID; + return program_id; } map > mesh_filter::GLRenderer::context_; @@ -379,12 +379,12 @@ void mesh_filter::GLRenderer::createGLContext() } // check if our thread is initialized - boost::thread::id threadID = boost::this_thread::get_id(); - map >::iterator contextIt = context_.find(threadID); + boost::thread::id thread_id = boost::this_thread::get_id(); + map >::iterator context_it = context_.find(thread_id); - if (contextIt == context_.end()) + if (context_it == context_.end()) { - context_[threadID] = std::pair(1, 0); + context_[thread_id] = std::pair(1, 0); glutInitWindowPosition(glutGet(GLUT_SCREEN_WIDTH) + 30000, 0); glutInitWindowSize(1, 1); @@ -394,10 +394,10 @@ void mesh_filter::GLRenderer::createGLContext() GLenum err = glewInit(); if (GLEW_OK != err) { - stringstream errorStream; - errorStream << "Unable to initialize GLEW: " << glewGetErrorString(err); + stringstream error_stream; + error_stream << "Unable to initialize GLEW: " << glewGetErrorString(err); - throw(runtime_error(errorStream.str())); + throw(runtime_error(error_stream.str())); } glutIconifyWindow(); glutHideWindow(); @@ -405,28 +405,28 @@ void mesh_filter::GLRenderer::createGLContext() for (int i = 0; i < 10; ++i) glutMainLoopEvent(); - context_[threadID] = std::pair(1, window_id); + context_[thread_id] = std::pair(1, window_id); } else - ++(contextIt->second.first); + ++(context_it->second.first); } void mesh_filter::GLRenderer::deleteGLContext() { boost::mutex::scoped_lock _(context_lock_); - boost::thread::id threadID = boost::this_thread::get_id(); - map >::iterator contextIt = context_.find(threadID); - if (contextIt == context_.end()) + boost::thread::id thread_id = boost::this_thread::get_id(); + map >::iterator context_it = context_.find(thread_id); + if (context_it == context_.end()) { - stringstream errorMsg; - errorMsg << "No OpenGL context exists for Thread " << threadID; - throw runtime_error(errorMsg.str()); + stringstream error_msg; + error_msg << "No OpenGL context exists for Thread " << thread_id; + throw runtime_error(error_msg.str()); } - if (--(contextIt->second.first) == 0) + if (--(context_it->second.first) == 0) { - glutDestroyWindow(contextIt->second.second); - context_.erase(contextIt); + glutDestroyWindow(context_it->second.second); + context_.erase(context_it); } } diff --git a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp index 83f28517c5..a3f123cf5d 100644 --- a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp +++ b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp @@ -324,9 +324,9 @@ void mesh_filter::MeshFilterBase::doFilter(const void* sensor_data, const int en glUniform3f(padding_coefficients_id, padding_coefficients[0], padding_coefficients[1], padding_coefficients[2]); Eigen::Isometry3d transform; - for (std::map::const_iterator meshIt = meshes_.begin(); meshIt != meshes_.end(); ++meshIt) - if (transform_callback_(meshIt->first, transform)) - meshIt->second->render(transform); + for (std::map::const_iterator mesh_it = meshes_.begin(); mesh_it != meshes_.end(); ++mesh_it) + if (transform_callback_(mesh_it->first, transform)) + mesh_it->second->render(transform); mesh_renderer_->end(); diff --git a/moveit_ros/perception/mesh_filter/src/sensor_model.cpp b/moveit_ros/perception/mesh_filter/src/sensor_model.cpp index 92920bb54e..d09cb649d9 100644 --- a/moveit_ros/perception/mesh_filter/src/sensor_model.cpp +++ b/moveit_ros/perception/mesh_filter/src/sensor_model.cpp @@ -160,8 +160,8 @@ void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(floa const float nf = near * far; const float f_n = far - near; - const float* depthEnd = depth + width_ * height_; - while (depth < depthEnd) + const float* depth_end = depth + width_ * height_; + while (depth < depth_end) { if (*depth != 0 && *depth != 1) *depth = nf / (far - *depth * f_n); @@ -218,10 +218,10 @@ void mesh_filter::SensorModel::Parameters::transformFilteredDepthToMetricDepth(f ++mmDepth; } #else - const float* depthEnd = depth + width_ * height_; + const float* depth_end = depth + width_ * height_; const float scale = far_clipping_plane_distance_ - near_clipping_plane_distance_; const float offset = near_clipping_plane_distance_; - while (depth < depthEnd) + while (depth < depth_end) { // 0 = on near clipping plane -> we used 0 to mark invalid points -> not visible // points on far clipping plane needs to be removed too diff --git a/moveit_ros/perception/mesh_filter/src/stereo_camera_model.cpp b/moveit_ros/perception/mesh_filter/src/stereo_camera_model.cpp index 9f0cfcd47a..5881faffd8 100644 --- a/moveit_ros/perception/mesh_filter/src/stereo_camera_model.cpp +++ b/moveit_ros/perception/mesh_filter/src/stereo_camera_model.cpp @@ -108,10 +108,11 @@ void mesh_filter::StereoCameraModel::Parameters::setFilterParameters(GLRenderer& renderer.setCameraParameters(fx_, fy_, cx_, cy_); } -const mesh_filter::StereoCameraModel::Parameters& mesh_filter::StereoCameraModel::RegisteredPSDKParams = +// NOLINTNEXTLINE(readability-identifier-naming) +const mesh_filter::StereoCameraModel::Parameters& mesh_filter::StereoCameraModel::REGISTERED_PSDK_PARAMS = mesh_filter::StereoCameraModel::Parameters(640, 480, 0.4, 10.0, 525, 525, 319.5, 239.5, 0.075, 0.125); -const string mesh_filter::StereoCameraModel::renderVertexShaderSource = +const string mesh_filter::StereoCameraModel::RENDER_VERTEX_SHADER_SOURCE = "#version 120\n" "uniform vec3 padding_coefficients;" "void main()" @@ -126,22 +127,22 @@ const string mesh_filter::StereoCameraModel::renderVertexShaderSource = " gl_Position.y = -gl_Position.y;" "}"; -const string mesh_filter::StereoCameraModel::renderFragmentShaderSource = "#version 120\n" - "void main()" - "{" - " gl_FragColor = gl_Color;" - "}"; - -const string mesh_filter::StereoCameraModel::filterVertexShaderSource = "#version 120\n" - "void main ()" - "{" - " gl_FrontColor = gl_Color;" - " gl_TexCoord[0] = gl_MultiTexCoord0;" - " gl_Position = gl_Vertex;" - " gl_Position.w = 1.0;" - "}"; - -const string mesh_filter::StereoCameraModel::filterFragmentShaderSource = +const string mesh_filter::StereoCameraModel::RENDER_FRAGMENT_SHADER_SOURCE = "#version 120\n" + "void main()" + "{" + " gl_FragColor = gl_Color;" + "}"; + +const string mesh_filter::StereoCameraModel::FILTER_VERTEX_SHADER_SOURCE = "#version 120\n" + "void main ()" + "{" + " gl_FrontColor = gl_Color;" + " gl_TexCoord[0] = gl_MultiTexCoord0;" + " gl_Position = gl_Vertex;" + " gl_Position.w = 1.0;" + "}"; + +const string mesh_filter::StereoCameraModel::FILTER_FRAGMENT_SHADER_SOURCE = "#version 120\n" "uniform sampler2D sensor;" "uniform sampler2D depth;" diff --git a/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp b/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp index fc050d8087..a452eb14f3 100644 --- a/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp +++ b/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp @@ -93,7 +93,7 @@ class MeshFilterTest : public testing::TestWithParam private: shapes::Mesh createMesh(double z) const; - bool transform_callback(MeshHandle handle, Isometry3d& transform) const; + bool transformCallback(MeshHandle handle, Isometry3d& transform) const; void getGroundTruth(unsigned int* labels, float* depth) const; const unsigned int width_; const unsigned int height_; @@ -118,7 +118,7 @@ MeshFilterTest::MeshFilterTest(unsigned width, unsigned height, double nea , shadow_(shadow) , epsilon_(epsilon) , sensor_parameters_(width, height, near_, far_, width >> 1, height >> 1, width >> 1, height >> 1, 0.1, 0.1) - , filter_(boost::bind(&MeshFilterTest::transform_callback, this, _1, _2), sensor_parameters_) + , filter_(boost::bind(&MeshFilterTest::transformCallback, this, _1, _2), sensor_parameters_) , sensor_data_(width_ * height_) , distance_(0.0) { @@ -136,12 +136,12 @@ MeshFilterTest::MeshFilterTest(unsigned width, unsigned height, double nea srand(0); Type t_near = near_ / FilterTraits::ToMetricScale; Type t_far = far_ / FilterTraits::ToMetricScale; - for (typename vector::iterator sIt = sensor_data_.begin(); sIt != sensor_data_.end(); ++sIt) + for (typename vector::iterator s_it = sensor_data_.begin(); s_it != sensor_data_.end(); ++s_it) { do { - *sIt = getRandomNumber(0.0, 10.0 / FilterTraits::ToMetricScale); - } while (*sIt == t_near || *sIt == t_far); + *s_it = getRandomNumber(0.0, 10.0 / FilterTraits::ToMetricScale); + } while (*s_it == t_near || *s_it == t_far); } } @@ -201,7 +201,7 @@ shapes::Mesh MeshFilterTest::createMesh(double z) const } template -bool MeshFilterTest::transform_callback(MeshHandle handle, Isometry3d& transform) const +bool MeshFilterTest::transformCallback(MeshHandle handle, Isometry3d& transform) const { transform = Isometry3d::Identity(); if (handle == handle_) @@ -245,9 +245,9 @@ void MeshFilterTest::getGroundTruth(unsigned int* labels, float* depth) co if (distance_ <= near_ || distance_ >= far_) { // no filtering is done -> no shadow values or label values - for (unsigned yIdx = 0, idx = 0; yIdx < height_; ++yIdx) + for (unsigned y_idx = 0, idx = 0; y_idx < height_; ++y_idx) { - for (unsigned xIdx = 0; xIdx < width_; ++xIdx, ++idx) + for (unsigned x_idx = 0; x_idx < width_; ++x_idx, ++idx) { depth[idx] = double(sensor_data_[idx]) * scale; if (depth[idx] < near_) @@ -264,9 +264,9 @@ void MeshFilterTest::getGroundTruth(unsigned int* labels, float* depth) co } else { - for (unsigned yIdx = 0, idx = 0; yIdx < height_; ++yIdx) + for (unsigned y_idx = 0, idx = 0; y_idx < height_; ++y_idx) { - for (unsigned xIdx = 0; xIdx < width_; ++xIdx, ++idx) + for (unsigned x_idx = 0; x_idx < width_; ++x_idx, ++idx) { depth[idx] = double(sensor_data_[idx]) * scale; diff --git a/moveit_ros/perception/occupancy_map_monitor/src/occupancy_map_monitor.cpp b/moveit_ros/perception/occupancy_map_monitor/src/occupancy_map_monitor.cpp index 1763736942..07c2d0080e 100644 --- a/moveit_ros/perception/occupancy_map_monitor/src/occupancy_map_monitor.cpp +++ b/moveit_ros/perception/occupancy_map_monitor/src/occupancy_map_monitor.cpp @@ -272,10 +272,10 @@ bool OccupancyMapMonitor::getShapeTransformCache(std::size_t index, const std::s { if (transform_cache_callback_) { - ShapeTransformCache tempCache; - if (transform_cache_callback_(target_frame, target_time, tempCache)) + ShapeTransformCache temp_cache; + if (transform_cache_callback_(target_frame, target_time, temp_cache)) { - for (ShapeTransformCache::iterator it = tempCache.begin(); it != tempCache.end(); ++it) + for (ShapeTransformCache::iterator it = temp_cache.begin(); it != temp_cache.end(); ++it) { std::map::const_iterator jt = mesh_handles_[index].find(it->first); if (jt == mesh_handles_[index].end()) diff --git a/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp b/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp index c31f3cdacd..6e47bebb4c 100644 --- a/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp +++ b/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp @@ -147,7 +147,7 @@ void point_containment_filter::ShapeMask::maskContainment(const sensor_msgs::Poi // compute a sphere that bounds the entire robot bodies::BoundingSphere bound; bodies::mergeBoundingSpheres(bspheres_, bound); - const double radiusSquared = bound.radius * bound.radius; + const double radius_squared = bound.radius * bound.radius; // we now decide which points we keep sensor_msgs::PointCloud2ConstIterator iter_x(data_in, "x"); @@ -164,7 +164,7 @@ void point_containment_filter::ShapeMask::maskContainment(const sensor_msgs::Poi int out = OUTSIDE; if (d < min_sensor_dist || d > max_sensor_dist) out = CLIP; - else if ((bound.center - pt).squaredNorm() < radiusSquared) + else if ((bound.center - pt).squaredNorm() < radius_squared) for (std::set::const_iterator it = bodies_.begin(); it != bodies_.end() && out == OUTSIDE; ++it) if (it->body->containsPoint(pt)) out = INSIDE; diff --git a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp index 057d621e6d..a037ffd4fd 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp +++ b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp @@ -184,9 +184,9 @@ void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2:: monitor_->setMapFrame(cloud_msg->header.frame_id); /* get transform for cloud into map frame */ - tf2::Stamped map_H_sensor; + tf2::Stamped map_h_sensor; if (monitor_->getMapFrame() == cloud_msg->header.frame_id) - map_H_sensor.setIdentity(); + map_h_sensor.setIdentity(); else { if (tf_buffer_) @@ -195,7 +195,7 @@ void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2:: { tf2::fromMsg( tf_buffer_->lookupTransform(monitor_->getMapFrame(), cloud_msg->header.frame_id, cloud_msg->header.stamp), - map_H_sensor); + map_h_sensor); } catch (tf2::TransformException& ex) { @@ -208,7 +208,7 @@ void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2:: } /* compute sensor origin in map frame */ - const tf2::Vector3& sensor_origin_tf = map_H_sensor.getOrigin(); + const tf2::Vector3& sensor_origin_tf = map_h_sensor.getOrigin(); octomap::point3d sensor_origin(sensor_origin_tf.getX(), sensor_origin_tf.getY(), sensor_origin_tf.getZ()); Eigen::Vector3d sensor_origin_eigen(sensor_origin_tf.getX(), sensor_origin_tf.getY(), sensor_origin_tf.getZ()); @@ -269,7 +269,7 @@ void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2:: if (!std::isnan(pt_iter[0]) && !std::isnan(pt_iter[1]) && !std::isnan(pt_iter[2])) { /* transform to map frame */ - tf2::Vector3 point_tf = map_H_sensor * tf2::Vector3(pt_iter[0], pt_iter[1], pt_iter[2]); + tf2::Vector3 point_tf = map_h_sensor * tf2::Vector3(pt_iter[0], pt_iter[1], pt_iter[2]); /* occupied cell at ray endpoint if ray is shorter than max range and this point isn't on a part of the robot*/ diff --git a/moveit_ros/perception/semantic_world/src/semantic_world.cpp b/moveit_ros/perception/semantic_world/src/semantic_world.cpp index 073ea9333c..b4a56ef05e 100644 --- a/moveit_ros/perception/semantic_world/src/semantic_world.cpp +++ b/moveit_ros/perception/semantic_world/src/semantic_world.cpp @@ -507,12 +507,15 @@ shapes::Mesh* SemanticWorld::orientPlanarPolygon(const shapes::Mesh& polygon) co // first get the normal of the first triangle of the input polygon Eigen::Vector3d vec1, vec2, vec3, normal; - int vIdx1 = polygon.triangles[0]; - int vIdx2 = polygon.triangles[1]; - int vIdx3 = polygon.triangles[2]; - vec1 = Eigen::Vector3d(polygon.vertices[vIdx1 * 3], polygon.vertices[vIdx1 * 3 + 1], polygon.vertices[vIdx1 * 3 + 2]); - vec2 = Eigen::Vector3d(polygon.vertices[vIdx2 * 3], polygon.vertices[vIdx2 * 3 + 1], polygon.vertices[vIdx2 * 3 + 2]); - vec3 = Eigen::Vector3d(polygon.vertices[vIdx3 * 3], polygon.vertices[vIdx3 * 3 + 1], polygon.vertices[vIdx3 * 3 + 2]); + int v_idx1 = polygon.triangles[0]; + int v_idx2 = polygon.triangles[1]; + int v_idx3 = polygon.triangles[2]; + vec1 = + Eigen::Vector3d(polygon.vertices[v_idx1 * 3], polygon.vertices[v_idx1 * 3 + 1], polygon.vertices[v_idx1 * 3 + 2]); + vec2 = + Eigen::Vector3d(polygon.vertices[v_idx2 * 3], polygon.vertices[v_idx2 * 3 + 1], polygon.vertices[v_idx2 * 3 + 2]); + vec3 = + Eigen::Vector3d(polygon.vertices[v_idx3 * 3], polygon.vertices[v_idx3 * 3 + 1], polygon.vertices[v_idx3 * 3 + 2]); vec2 -= vec1; vec3 -= vec1; normal = vec3.cross(vec2); @@ -530,18 +533,18 @@ shapes::Mesh* SemanticWorld::orientPlanarPolygon(const shapes::Mesh& polygon) co // copy the first set of triangles memcpy(solid->triangles, polygon.triangles, polygon.triangle_count * 3 * sizeof(unsigned int)); - for (unsigned tIdx = 0; tIdx < polygon.triangle_count; ++tIdx) + for (unsigned t_idx = 0; t_idx < polygon.triangle_count; ++t_idx) { - int vIdx1 = polygon.triangles[tIdx * 3]; - int vIdx2 = polygon.triangles[tIdx * 3 + 1]; - int vIdx3 = polygon.triangles[tIdx * 3 + 2]; + int v_idx1 = polygon.triangles[t_idx * 3]; + int v_idx2 = polygon.triangles[t_idx * 3 + 1]; + int v_idx3 = polygon.triangles[t_idx * 3 + 2]; - vec1 = - Eigen::Vector3d(polygon.vertices[vIdx1 * 3], polygon.vertices[vIdx1 * 3 + 1], polygon.vertices[vIdx1 * 3 + 2]); - vec2 = - Eigen::Vector3d(polygon.vertices[vIdx2 * 3], polygon.vertices[vIdx2 * 3 + 1], polygon.vertices[vIdx2 * 3 + 2]); - vec3 = - Eigen::Vector3d(polygon.vertices[vIdx3 * 3], polygon.vertices[vIdx3 * 3 + 1], polygon.vertices[vIdx3 * 3 + 2]); + vec1 = Eigen::Vector3d(polygon.vertices[v_idx1 * 3], polygon.vertices[v_idx1 * 3 + 1], + polygon.vertices[v_idx1 * 3 + 2]); + vec2 = Eigen::Vector3d(polygon.vertices[v_idx2 * 3], polygon.vertices[v_idx2 * 3 + 1], + polygon.vertices[v_idx2 * 3 + 2]); + vec3 = Eigen::Vector3d(polygon.vertices[v_idx3 * 3], polygon.vertices[v_idx3 * 3 + 1], + polygon.vertices[v_idx3 * 3 + 2]); vec2 -= vec1; vec3 -= vec1; @@ -549,7 +552,7 @@ shapes::Mesh* SemanticWorld::orientPlanarPolygon(const shapes::Mesh& polygon) co Eigen::Vector3d triangle_normal = vec2.cross(vec1); if (triangle_normal.dot(normal) < 0.0) - std::swap(solid->triangles[tIdx * 3 + 1], solid->triangles[tIdx * 3 + 2]); + std::swap(solid->triangles[t_idx * 3 + 1], solid->triangles[t_idx * 3 + 2]); } return solid; } @@ -561,12 +564,15 @@ shapes::Mesh* SemanticWorld::createSolidMeshFromPlanarPolygon(const shapes::Mesh // first get the normal of the first triangle of the input polygon Eigen::Vector3d vec1, vec2, vec3, normal; - int vIdx1 = polygon.triangles[0]; - int vIdx2 = polygon.triangles[1]; - int vIdx3 = polygon.triangles[2]; - vec1 = Eigen::Vector3d(polygon.vertices[vIdx1 * 3], polygon.vertices[vIdx1 * 3 + 1], polygon.vertices[vIdx1 * 3 + 2]); - vec2 = Eigen::Vector3d(polygon.vertices[vIdx2 * 3], polygon.vertices[vIdx2 * 3 + 1], polygon.vertices[vIdx2 * 3 + 2]); - vec3 = Eigen::Vector3d(polygon.vertices[vIdx3 * 3], polygon.vertices[vIdx3 * 3 + 1], polygon.vertices[vIdx3 * 3 + 2]); + int v_idx1 = polygon.triangles[0]; + int v_idx2 = polygon.triangles[1]; + int v_idx3 = polygon.triangles[2]; + vec1 = + Eigen::Vector3d(polygon.vertices[v_idx1 * 3], polygon.vertices[v_idx1 * 3 + 1], polygon.vertices[v_idx1 * 3 + 2]); + vec2 = + Eigen::Vector3d(polygon.vertices[v_idx2 * 3], polygon.vertices[v_idx2 * 3 + 1], polygon.vertices[v_idx2 * 3 + 2]); + vec3 = + Eigen::Vector3d(polygon.vertices[v_idx3 * 3], polygon.vertices[v_idx3 * 3 + 1], polygon.vertices[v_idx3 * 3 + 2]); vec2 -= vec1; vec3 -= vec1; normal = vec3.cross(vec2); @@ -588,22 +594,22 @@ shapes::Mesh* SemanticWorld::createSolidMeshFromPlanarPolygon(const shapes::Mesh // copy the first set of triangles memcpy(solid->triangles, polygon.triangles, polygon.triangle_count * 3 * sizeof(unsigned int)); - for (unsigned tIdx = 0; tIdx < polygon.triangle_count; ++tIdx) + for (unsigned t_idx = 0; t_idx < polygon.triangle_count; ++t_idx) { - solid->triangles[(tIdx + polygon.triangle_count) * 3 + 0] = solid->triangles[tIdx * 3 + 0] + polygon.vertex_count; - solid->triangles[(tIdx + polygon.triangle_count) * 3 + 1] = solid->triangles[tIdx * 3 + 1] + polygon.vertex_count; - solid->triangles[(tIdx + polygon.triangle_count) * 3 + 2] = solid->triangles[tIdx * 3 + 2] + polygon.vertex_count; + solid->triangles[(t_idx + polygon.triangle_count) * 3 + 0] = solid->triangles[t_idx * 3 + 0] + polygon.vertex_count; + solid->triangles[(t_idx + polygon.triangle_count) * 3 + 1] = solid->triangles[t_idx * 3 + 1] + polygon.vertex_count; + solid->triangles[(t_idx + polygon.triangle_count) * 3 + 2] = solid->triangles[t_idx * 3 + 2] + polygon.vertex_count; - int vIdx1 = polygon.triangles[tIdx * 3]; - int vIdx2 = polygon.triangles[tIdx * 3 + 1]; - int vIdx3 = polygon.triangles[tIdx * 3 + 2]; + int v_idx1 = polygon.triangles[t_idx * 3]; + int v_idx2 = polygon.triangles[t_idx * 3 + 1]; + int v_idx3 = polygon.triangles[t_idx * 3 + 2]; - vec1 = - Eigen::Vector3d(polygon.vertices[vIdx1 * 3], polygon.vertices[vIdx1 * 3 + 1], polygon.vertices[vIdx1 * 3 + 2]); - vec2 = - Eigen::Vector3d(polygon.vertices[vIdx2 * 3], polygon.vertices[vIdx2 * 3 + 1], polygon.vertices[vIdx2 * 3 + 2]); - vec3 = - Eigen::Vector3d(polygon.vertices[vIdx3 * 3], polygon.vertices[vIdx3 * 3 + 1], polygon.vertices[vIdx3 * 3 + 2]); + vec1 = Eigen::Vector3d(polygon.vertices[v_idx1 * 3], polygon.vertices[v_idx1 * 3 + 1], + polygon.vertices[v_idx1 * 3 + 2]); + vec2 = Eigen::Vector3d(polygon.vertices[v_idx2 * 3], polygon.vertices[v_idx2 * 3 + 1], + polygon.vertices[v_idx2 * 3 + 2]); + vec3 = Eigen::Vector3d(polygon.vertices[v_idx3 * 3], polygon.vertices[v_idx3 * 3 + 1], + polygon.vertices[v_idx3 * 3 + 2]); vec2 -= vec1; vec3 -= vec1; @@ -611,17 +617,17 @@ shapes::Mesh* SemanticWorld::createSolidMeshFromPlanarPolygon(const shapes::Mesh Eigen::Vector3d triangle_normal = vec2.cross(vec1); if (triangle_normal.dot(normal) < 0.0) - std::swap(solid->triangles[tIdx * 3 + 1], solid->triangles[tIdx * 3 + 2]); + std::swap(solid->triangles[t_idx * 3 + 1], solid->triangles[t_idx * 3 + 2]); else - std::swap(solid->triangles[(tIdx + polygon.triangle_count) * 3 + 1], - solid->triangles[(tIdx + polygon.triangle_count) * 3 + 2]); + std::swap(solid->triangles[(t_idx + polygon.triangle_count) * 3 + 1], + solid->triangles[(t_idx + polygon.triangle_count) * 3 + 2]); } - for (unsigned vIdx = 0; vIdx < polygon.vertex_count; ++vIdx) + for (unsigned v_idx = 0; v_idx < polygon.vertex_count; ++v_idx) { - solid->vertices[(vIdx + polygon.vertex_count) * 3 + 0] = solid->vertices[vIdx * 3 + 0] - thickness * normal[0]; - solid->vertices[(vIdx + polygon.vertex_count) * 3 + 1] = solid->vertices[vIdx * 3 + 1] - thickness * normal[1]; - solid->vertices[(vIdx + polygon.vertex_count) * 3 + 2] = solid->vertices[vIdx * 3 + 2] - thickness * normal[2]; + solid->vertices[(v_idx + polygon.vertex_count) * 3 + 0] = solid->vertices[v_idx * 3 + 0] - thickness * normal[0]; + solid->vertices[(v_idx + polygon.vertex_count) * 3 + 1] = solid->vertices[v_idx * 3 + 1] - thickness * normal[1]; + solid->vertices[(v_idx + polygon.vertex_count) * 3 + 2] = solid->vertices[v_idx * 3 + 2] - thickness * normal[2]; } return solid; diff --git a/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp b/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp index 19e0f42767..50d2941091 100644 --- a/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp +++ b/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp @@ -64,8 +64,8 @@ int main(int argc, char** argv) ros::Publisher pub_markers = nh.advertise("visualization_marker_array", 10); std::cout << "\nListening for planning scene...\nType the number of spheres to generate and press Enter: " << std::endl; - int N; - std::cin >> N; + int num_spheres; + std::cin >> num_spheres; planning_scene::PlanningScenePtr scene = psm.getPlanningScene(); std::vector aabb; @@ -108,7 +108,7 @@ int main(int argc, char** argv) color.b = 0.0f; color.a = 1.0f; - for (int i = 0; i < N; ++i) + for (int i = 0; i < num_spheres; ++i) { t.translation() = Eigen::Vector3d(rng.uniformReal(aabb[0], aabb[1]), rng.uniformReal(aabb[2], aabb[3]), rng.uniformReal(aabb[4], aabb[5])); @@ -119,7 +119,7 @@ int main(int argc, char** argv) if (res.collision) { points.push_back(t.translation()); - if (points.size() - published >= 100 || (points.size() > published && i + 1 >= N)) + if (points.size() - published >= 100 || (points.size() > published && i + 1 >= num_spheres)) { arr.markers.clear(); for (std::size_t k = published; k < points.size(); ++k) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp index 453a56f691..9022118758 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp @@ -51,15 +51,14 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning { } - virtual std::string getDescription() const + std::string getDescription() const override { return "Add Time Optimal Parameterization"; } - virtual bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const + bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, + std::vector& added_path_index) const override { bool result = planner(planning_scene, req, res); if (result && res.trajectory_) @@ -75,7 +74,7 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning } }; -} // namespace default_planners_request_adapters +} // namespace default_planner_request_adapters CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::AddTimeOptimalParameterization, planning_request_adapter::PlanningRequestAdapter); diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index 49d5bed29c..c675674ce1 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -1398,7 +1398,7 @@ void PlanningSceneMonitor::configureDefaultPadding() } // Ensure no leading slash creates a bad param server address - static const std::string robot_description = + const std::string robot_description = (robot_description_[0] == '/') ? robot_description_.substr(1) : robot_description_; nh_.param(robot_description + "_planning/default_robot_padding", default_robot_padd_, 0.0); diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index 1f4a48b46e..0c8d00e27f 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -418,9 +418,9 @@ void TrajectoryExecutionManager::continuousExecutionThread() while (uit != used_handles.end()) if ((*uit)->getLastExecutionStatus() != moveit_controller_manager::ExecutionStatus::RUNNING) { - std::set::iterator toErase = uit; + std::set::iterator to_erase = uit; ++uit; - used_handles.erase(toErase); + used_handles.erase(to_erase); } else ++uit; @@ -1037,15 +1037,15 @@ bool TrajectoryExecutionManager::configure(TrajectoryExecutionContext& context, } std::set actuated_joints; - auto isActuated = [this](const std::string& joint_name) -> bool { + auto is_actuated = [this](const std::string& joint_name) -> bool { const robot_model::JointModel* jm = robot_model_->getJointModel(joint_name); return (jm && !jm->isPassive() && !jm->getMimic() && jm->getType() != robot_model::JointModel::FIXED); }; for (const std::string& joint_name : trajectory.multi_dof_joint_trajectory.joint_names) - if (isActuated(joint_name)) + if (is_actuated(joint_name)) actuated_joints.insert(joint_name); for (const std::string& joint_name : trajectory.joint_trajectory.joint_names) - if (isActuated(joint_name)) + if (is_actuated(joint_name)) actuated_joints.insert(joint_name); if (actuated_joints.empty()) 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 9b042496e2..fd0fddc971 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 @@ -493,9 +493,9 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return jt->second.at(0); // or return an error - static const geometry_msgs::PoseStamped unknown; + static const geometry_msgs::PoseStamped UNKNOWN; ROS_ERROR_NAMED("move_group_interface", "Pose for end-effector '%s' not known.", eef.c_str()); - return unknown; + return UNKNOWN; } const std::vector& getPoseTargets(const std::string& end_effector_link) const @@ -508,9 +508,9 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return jt->second; // or return an error - static const std::vector empty; + static const std::vector EMPTY; ROS_ERROR_NAMED("move_group_interface", "Poses for end-effector '%s' are not known.", eef.c_str()); - return empty; + return EMPTY; } void setPoseReferenceFrame(const std::string& pose_reference_frame) diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp index 30eb33f56d..3b63a48967 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp @@ -524,135 +524,139 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer static void wrap_move_group_interface() { - bp::class_ MoveGroupInterfaceClass( + bp::class_ move_group_interface_class( "MoveGroupInterface", bp::init>()); - MoveGroupInterfaceClass.def("async_move", &MoveGroupInterfaceWrapper::asyncMovePython); - MoveGroupInterfaceClass.def("move", &MoveGroupInterfaceWrapper::movePython); - MoveGroupInterfaceClass.def("execute", &MoveGroupInterfaceWrapper::executePython); - MoveGroupInterfaceClass.def("async_execute", &MoveGroupInterfaceWrapper::asyncExecutePython); + move_group_interface_class.def("async_move", &MoveGroupInterfaceWrapper::asyncMovePython); + move_group_interface_class.def("move", &MoveGroupInterfaceWrapper::movePython); + move_group_interface_class.def("execute", &MoveGroupInterfaceWrapper::executePython); + move_group_interface_class.def("async_execute", &MoveGroupInterfaceWrapper::asyncExecutePython); moveit::planning_interface::MoveItErrorCode (MoveGroupInterfaceWrapper::*pick_1)(const std::string&, bool) = &MoveGroupInterfaceWrapper::pick; - MoveGroupInterfaceClass.def("pick", pick_1); - MoveGroupInterfaceClass.def("pick", &MoveGroupInterfaceWrapper::pickGrasp); - MoveGroupInterfaceClass.def("pick", &MoveGroupInterfaceWrapper::pickGrasps); - MoveGroupInterfaceClass.def("place", &MoveGroupInterfaceWrapper::placePose); - MoveGroupInterfaceClass.def("place", &MoveGroupInterfaceWrapper::placeLocation); - MoveGroupInterfaceClass.def("place", &MoveGroupInterfaceWrapper::placeAnywhere); - MoveGroupInterfaceClass.def("stop", &MoveGroupInterfaceWrapper::stop); + move_group_interface_class.def("pick", pick_1); + move_group_interface_class.def("pick", &MoveGroupInterfaceWrapper::pickGrasp); + move_group_interface_class.def("pick", &MoveGroupInterfaceWrapper::pickGrasps); + move_group_interface_class.def("place", &MoveGroupInterfaceWrapper::placePose); + move_group_interface_class.def("place", &MoveGroupInterfaceWrapper::placeLocation); + move_group_interface_class.def("place", &MoveGroupInterfaceWrapper::placeAnywhere); + move_group_interface_class.def("stop", &MoveGroupInterfaceWrapper::stop); - MoveGroupInterfaceClass.def("get_name", &MoveGroupInterfaceWrapper::getNameCStr); - MoveGroupInterfaceClass.def("get_planning_frame", &MoveGroupInterfaceWrapper::getPlanningFrameCStr); - MoveGroupInterfaceClass.def("get_interface_description", &MoveGroupInterfaceWrapper::getInterfaceDescriptionPython); + move_group_interface_class.def("get_name", &MoveGroupInterfaceWrapper::getNameCStr); + move_group_interface_class.def("get_planning_frame", &MoveGroupInterfaceWrapper::getPlanningFrameCStr); + move_group_interface_class.def("get_interface_description", + &MoveGroupInterfaceWrapper::getInterfaceDescriptionPython); - MoveGroupInterfaceClass.def("get_active_joints", &MoveGroupInterfaceWrapper::getActiveJointsList); - MoveGroupInterfaceClass.def("get_joints", &MoveGroupInterfaceWrapper::getJointsList); - MoveGroupInterfaceClass.def("get_variable_count", &MoveGroupInterfaceWrapper::getVariableCount); - MoveGroupInterfaceClass.def("allow_looking", &MoveGroupInterfaceWrapper::allowLooking); - MoveGroupInterfaceClass.def("allow_replanning", &MoveGroupInterfaceWrapper::allowReplanning); + move_group_interface_class.def("get_active_joints", &MoveGroupInterfaceWrapper::getActiveJointsList); + move_group_interface_class.def("get_joints", &MoveGroupInterfaceWrapper::getJointsList); + move_group_interface_class.def("get_variable_count", &MoveGroupInterfaceWrapper::getVariableCount); + move_group_interface_class.def("allow_looking", &MoveGroupInterfaceWrapper::allowLooking); + move_group_interface_class.def("allow_replanning", &MoveGroupInterfaceWrapper::allowReplanning); - MoveGroupInterfaceClass.def("set_pose_reference_frame", &MoveGroupInterfaceWrapper::setPoseReferenceFrame); + move_group_interface_class.def("set_pose_reference_frame", &MoveGroupInterfaceWrapper::setPoseReferenceFrame); - MoveGroupInterfaceClass.def("set_pose_reference_frame", &MoveGroupInterfaceWrapper::setPoseReferenceFrame); - MoveGroupInterfaceClass.def("set_end_effector_link", &MoveGroupInterfaceWrapper::setEndEffectorLink); - MoveGroupInterfaceClass.def("get_end_effector_link", &MoveGroupInterfaceWrapper::getEndEffectorLinkCStr); - MoveGroupInterfaceClass.def("get_pose_reference_frame", &MoveGroupInterfaceWrapper::getPoseReferenceFrameCStr); + move_group_interface_class.def("set_pose_reference_frame", &MoveGroupInterfaceWrapper::setPoseReferenceFrame); + move_group_interface_class.def("set_end_effector_link", &MoveGroupInterfaceWrapper::setEndEffectorLink); + move_group_interface_class.def("get_end_effector_link", &MoveGroupInterfaceWrapper::getEndEffectorLinkCStr); + move_group_interface_class.def("get_pose_reference_frame", &MoveGroupInterfaceWrapper::getPoseReferenceFrameCStr); - MoveGroupInterfaceClass.def("set_pose_target", &MoveGroupInterfaceWrapper::setPoseTargetPython); + move_group_interface_class.def("set_pose_target", &MoveGroupInterfaceWrapper::setPoseTargetPython); - MoveGroupInterfaceClass.def("set_pose_targets", &MoveGroupInterfaceWrapper::setPoseTargetsPython); + move_group_interface_class.def("set_pose_targets", &MoveGroupInterfaceWrapper::setPoseTargetsPython); - MoveGroupInterfaceClass.def("set_position_target", &MoveGroupInterfaceWrapper::setPositionTarget); - MoveGroupInterfaceClass.def("set_rpy_target", &MoveGroupInterfaceWrapper::setRPYTarget); - MoveGroupInterfaceClass.def("set_orientation_target", &MoveGroupInterfaceWrapper::setOrientationTarget); + move_group_interface_class.def("set_position_target", &MoveGroupInterfaceWrapper::setPositionTarget); + move_group_interface_class.def("set_rpy_target", &MoveGroupInterfaceWrapper::setRPYTarget); + move_group_interface_class.def("set_orientation_target", &MoveGroupInterfaceWrapper::setOrientationTarget); - MoveGroupInterfaceClass.def("get_current_pose", &MoveGroupInterfaceWrapper::getCurrentPosePython); - MoveGroupInterfaceClass.def("get_current_rpy", &MoveGroupInterfaceWrapper::getCurrentRPYPython); + move_group_interface_class.def("get_current_pose", &MoveGroupInterfaceWrapper::getCurrentPosePython); + move_group_interface_class.def("get_current_rpy", &MoveGroupInterfaceWrapper::getCurrentRPYPython); - MoveGroupInterfaceClass.def("get_random_pose", &MoveGroupInterfaceWrapper::getRandomPosePython); + move_group_interface_class.def("get_random_pose", &MoveGroupInterfaceWrapper::getRandomPosePython); - MoveGroupInterfaceClass.def("clear_pose_target", &MoveGroupInterfaceWrapper::clearPoseTarget); - MoveGroupInterfaceClass.def("clear_pose_targets", &MoveGroupInterfaceWrapper::clearPoseTargets); + move_group_interface_class.def("clear_pose_target", &MoveGroupInterfaceWrapper::clearPoseTarget); + move_group_interface_class.def("clear_pose_targets", &MoveGroupInterfaceWrapper::clearPoseTargets); - MoveGroupInterfaceClass.def("set_joint_value_target", &MoveGroupInterfaceWrapper::setJointValueTargetPythonIterable); - MoveGroupInterfaceClass.def("set_joint_value_target", &MoveGroupInterfaceWrapper::setJointValueTargetPythonDict); + move_group_interface_class.def("set_joint_value_target", + &MoveGroupInterfaceWrapper::setJointValueTargetPythonIterable); + move_group_interface_class.def("set_joint_value_target", &MoveGroupInterfaceWrapper::setJointValueTargetPythonDict); - MoveGroupInterfaceClass.def("set_joint_value_target", - &MoveGroupInterfaceWrapper::setJointValueTargetPerJointPythonList); - bool (MoveGroupInterfaceWrapper::*setJointValueTarget_4)(const std::string&, double) = + move_group_interface_class.def("set_joint_value_target", + &MoveGroupInterfaceWrapper::setJointValueTargetPerJointPythonList); + bool (MoveGroupInterfaceWrapper::*set_joint_value_target_4)(const std::string&, double) = &MoveGroupInterfaceWrapper::setJointValueTarget; - MoveGroupInterfaceClass.def("set_joint_value_target", setJointValueTarget_4); - MoveGroupInterfaceClass.def("set_state_value_target", &MoveGroupInterfaceWrapper::setStateValueTarget); + move_group_interface_class.def("set_joint_value_target", set_joint_value_target_4); + move_group_interface_class.def("set_state_value_target", &MoveGroupInterfaceWrapper::setStateValueTarget); - MoveGroupInterfaceClass.def("set_joint_value_target_from_pose", - &MoveGroupInterfaceWrapper::setJointValueTargetFromPosePython); - MoveGroupInterfaceClass.def("set_joint_value_target_from_pose_stamped", - &MoveGroupInterfaceWrapper::setJointValueTargetFromPoseStampedPython); - MoveGroupInterfaceClass.def("set_joint_value_target_from_joint_state_message", - &MoveGroupInterfaceWrapper::setJointValueTargetFromJointStatePython); + move_group_interface_class.def("set_joint_value_target_from_pose", + &MoveGroupInterfaceWrapper::setJointValueTargetFromPosePython); + move_group_interface_class.def("set_joint_value_target_from_pose_stamped", + &MoveGroupInterfaceWrapper::setJointValueTargetFromPoseStampedPython); + move_group_interface_class.def("set_joint_value_target_from_joint_state_message", + &MoveGroupInterfaceWrapper::setJointValueTargetFromJointStatePython); - MoveGroupInterfaceClass.def("get_joint_value_target", &MoveGroupInterfaceWrapper::getJointValueTargetPythonList); + move_group_interface_class.def("get_joint_value_target", &MoveGroupInterfaceWrapper::getJointValueTargetPythonList); - MoveGroupInterfaceClass.def("set_named_target", &MoveGroupInterfaceWrapper::setNamedTarget); - MoveGroupInterfaceClass.def("set_random_target", &MoveGroupInterfaceWrapper::setRandomTarget); + move_group_interface_class.def("set_named_target", &MoveGroupInterfaceWrapper::setNamedTarget); + move_group_interface_class.def("set_random_target", &MoveGroupInterfaceWrapper::setRandomTarget); - void (MoveGroupInterfaceWrapper::*rememberJointValues_2)(const std::string&) = + void (MoveGroupInterfaceWrapper::*remember_joint_values_2)(const std::string&) = &MoveGroupInterfaceWrapper::rememberJointValues; - MoveGroupInterfaceClass.def("remember_joint_values", rememberJointValues_2); + move_group_interface_class.def("remember_joint_values", remember_joint_values_2); - MoveGroupInterfaceClass.def("remember_joint_values", &MoveGroupInterfaceWrapper::rememberJointValuesFromPythonList); + move_group_interface_class.def("remember_joint_values", + &MoveGroupInterfaceWrapper::rememberJointValuesFromPythonList); - MoveGroupInterfaceClass.def("start_state_monitor", &MoveGroupInterfaceWrapper::startStateMonitor); - MoveGroupInterfaceClass.def("get_current_joint_values", &MoveGroupInterfaceWrapper::getCurrentJointValuesList); - MoveGroupInterfaceClass.def("get_random_joint_values", &MoveGroupInterfaceWrapper::getRandomJointValuesList); - MoveGroupInterfaceClass.def("get_remembered_joint_values", - &MoveGroupInterfaceWrapper::getRememberedJointValuesPython); + move_group_interface_class.def("start_state_monitor", &MoveGroupInterfaceWrapper::startStateMonitor); + move_group_interface_class.def("get_current_joint_values", &MoveGroupInterfaceWrapper::getCurrentJointValuesList); + move_group_interface_class.def("get_random_joint_values", &MoveGroupInterfaceWrapper::getRandomJointValuesList); + move_group_interface_class.def("get_remembered_joint_values", + &MoveGroupInterfaceWrapper::getRememberedJointValuesPython); - MoveGroupInterfaceClass.def("forget_joint_values", &MoveGroupInterfaceWrapper::forgetJointValues); + move_group_interface_class.def("forget_joint_values", &MoveGroupInterfaceWrapper::forgetJointValues); - MoveGroupInterfaceClass.def("get_goal_joint_tolerance", &MoveGroupInterfaceWrapper::getGoalJointTolerance); - MoveGroupInterfaceClass.def("get_goal_position_tolerance", &MoveGroupInterfaceWrapper::getGoalPositionTolerance); - MoveGroupInterfaceClass.def("get_goal_orientation_tolerance", - &MoveGroupInterfaceWrapper::getGoalOrientationTolerance); + move_group_interface_class.def("get_goal_joint_tolerance", &MoveGroupInterfaceWrapper::getGoalJointTolerance); + move_group_interface_class.def("get_goal_position_tolerance", &MoveGroupInterfaceWrapper::getGoalPositionTolerance); + move_group_interface_class.def("get_goal_orientation_tolerance", + &MoveGroupInterfaceWrapper::getGoalOrientationTolerance); - MoveGroupInterfaceClass.def("set_goal_joint_tolerance", &MoveGroupInterfaceWrapper::setGoalJointTolerance); - MoveGroupInterfaceClass.def("set_goal_position_tolerance", &MoveGroupInterfaceWrapper::setGoalPositionTolerance); - MoveGroupInterfaceClass.def("set_goal_orientation_tolerance", - &MoveGroupInterfaceWrapper::setGoalOrientationTolerance); - MoveGroupInterfaceClass.def("set_goal_tolerance", &MoveGroupInterfaceWrapper::setGoalTolerance); + move_group_interface_class.def("set_goal_joint_tolerance", &MoveGroupInterfaceWrapper::setGoalJointTolerance); + move_group_interface_class.def("set_goal_position_tolerance", &MoveGroupInterfaceWrapper::setGoalPositionTolerance); + move_group_interface_class.def("set_goal_orientation_tolerance", + &MoveGroupInterfaceWrapper::setGoalOrientationTolerance); + move_group_interface_class.def("set_goal_tolerance", &MoveGroupInterfaceWrapper::setGoalTolerance); - MoveGroupInterfaceClass.def("set_start_state_to_current_state", - &MoveGroupInterfaceWrapper::setStartStateToCurrentState); - MoveGroupInterfaceClass.def("set_start_state", &MoveGroupInterfaceWrapper::setStartStatePython); + move_group_interface_class.def("set_start_state_to_current_state", + &MoveGroupInterfaceWrapper::setStartStateToCurrentState); + move_group_interface_class.def("set_start_state", &MoveGroupInterfaceWrapper::setStartStatePython); - bool (MoveGroupInterfaceWrapper::*setPathConstraints_1)(const std::string&) = + bool (MoveGroupInterfaceWrapper::*set_path_constraints_1)(const std::string&) = &MoveGroupInterfaceWrapper::setPathConstraints; - MoveGroupInterfaceClass.def("set_path_constraints", setPathConstraints_1); - MoveGroupInterfaceClass.def("set_path_constraints_from_msg", &MoveGroupInterfaceWrapper::setPathConstraintsFromMsg); - MoveGroupInterfaceClass.def("get_path_constraints", &MoveGroupInterfaceWrapper::getPathConstraintsPython); - MoveGroupInterfaceClass.def("clear_path_constraints", &MoveGroupInterfaceWrapper::clearPathConstraints); - MoveGroupInterfaceClass.def("get_known_constraints", &MoveGroupInterfaceWrapper::getKnownConstraintsList); - MoveGroupInterfaceClass.def("set_constraints_database", &MoveGroupInterfaceWrapper::setConstraintsDatabase); - MoveGroupInterfaceClass.def("set_workspace", &MoveGroupInterfaceWrapper::setWorkspace); - MoveGroupInterfaceClass.def("set_planning_time", &MoveGroupInterfaceWrapper::setPlanningTime); - MoveGroupInterfaceClass.def("get_planning_time", &MoveGroupInterfaceWrapper::getPlanningTime); - MoveGroupInterfaceClass.def("set_max_velocity_scaling_factor", - &MoveGroupInterfaceWrapper::setMaxVelocityScalingFactor); - MoveGroupInterfaceClass.def("set_max_acceleration_scaling_factor", - &MoveGroupInterfaceWrapper::setMaxAccelerationScalingFactor); - MoveGroupInterfaceClass.def("set_planner_id", &MoveGroupInterfaceWrapper::setPlannerId); - MoveGroupInterfaceClass.def("set_num_planning_attempts", &MoveGroupInterfaceWrapper::setNumPlanningAttempts); - MoveGroupInterfaceClass.def("compute_plan", &MoveGroupInterfaceWrapper::getPlanPython); - MoveGroupInterfaceClass.def("compute_cartesian_path", &MoveGroupInterfaceWrapper::computeCartesianPathPython); - MoveGroupInterfaceClass.def("compute_cartesian_path", - &MoveGroupInterfaceWrapper::computeCartesianPathConstrainedPython); - MoveGroupInterfaceClass.def("set_support_surface_name", &MoveGroupInterfaceWrapper::setSupportSurfaceName); - MoveGroupInterfaceClass.def("attach_object", &MoveGroupInterfaceWrapper::attachObjectPython); - MoveGroupInterfaceClass.def("detach_object", &MoveGroupInterfaceWrapper::detachObject); - MoveGroupInterfaceClass.def("retime_trajectory", &MoveGroupInterfaceWrapper::retimeTrajectory); - MoveGroupInterfaceClass.def("get_named_targets", &MoveGroupInterfaceWrapper::getNamedTargetsPython); - MoveGroupInterfaceClass.def("get_named_target_values", &MoveGroupInterfaceWrapper::getNamedTargetValuesPython); - MoveGroupInterfaceClass.def("get_current_state_bounded", &MoveGroupInterfaceWrapper::getCurrentStateBoundedPython); + move_group_interface_class.def("set_path_constraints", set_path_constraints_1); + move_group_interface_class.def("set_path_constraints_from_msg", + &MoveGroupInterfaceWrapper::setPathConstraintsFromMsg); + move_group_interface_class.def("get_path_constraints", &MoveGroupInterfaceWrapper::getPathConstraintsPython); + move_group_interface_class.def("clear_path_constraints", &MoveGroupInterfaceWrapper::clearPathConstraints); + move_group_interface_class.def("get_known_constraints", &MoveGroupInterfaceWrapper::getKnownConstraintsList); + move_group_interface_class.def("set_constraints_database", &MoveGroupInterfaceWrapper::setConstraintsDatabase); + move_group_interface_class.def("set_workspace", &MoveGroupInterfaceWrapper::setWorkspace); + move_group_interface_class.def("set_planning_time", &MoveGroupInterfaceWrapper::setPlanningTime); + move_group_interface_class.def("get_planning_time", &MoveGroupInterfaceWrapper::getPlanningTime); + move_group_interface_class.def("set_max_velocity_scaling_factor", + &MoveGroupInterfaceWrapper::setMaxVelocityScalingFactor); + move_group_interface_class.def("set_max_acceleration_scaling_factor", + &MoveGroupInterfaceWrapper::setMaxAccelerationScalingFactor); + move_group_interface_class.def("set_planner_id", &MoveGroupInterfaceWrapper::setPlannerId); + move_group_interface_class.def("set_num_planning_attempts", &MoveGroupInterfaceWrapper::setNumPlanningAttempts); + move_group_interface_class.def("compute_plan", &MoveGroupInterfaceWrapper::getPlanPython); + move_group_interface_class.def("compute_cartesian_path", &MoveGroupInterfaceWrapper::computeCartesianPathPython); + move_group_interface_class.def("compute_cartesian_path", + &MoveGroupInterfaceWrapper::computeCartesianPathConstrainedPython); + move_group_interface_class.def("set_support_surface_name", &MoveGroupInterfaceWrapper::setSupportSurfaceName); + move_group_interface_class.def("attach_object", &MoveGroupInterfaceWrapper::attachObjectPython); + move_group_interface_class.def("detach_object", &MoveGroupInterfaceWrapper::detachObject); + move_group_interface_class.def("retime_trajectory", &MoveGroupInterfaceWrapper::retimeTrajectory); + move_group_interface_class.def("get_named_targets", &MoveGroupInterfaceWrapper::getNamedTargetsPython); + move_group_interface_class.def("get_named_target_values", &MoveGroupInterfaceWrapper::getNamedTargetValuesPython); + move_group_interface_class.def("get_current_state_bounded", &MoveGroupInterfaceWrapper::getCurrentStateBoundedPython); } } // namespace planning_interface } // namespace moveit diff --git a/moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp b/moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp index 1ba8892bb1..e1e825b0b0 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp +++ b/moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp @@ -114,16 +114,16 @@ class PlanningSceneInterfaceWrapper : protected py_bindings_tools::ROScppInitial static void wrap_planning_scene_interface() { - bp::class_ PlanningSceneClass("PlanningSceneInterface", - bp::init>()); - - PlanningSceneClass.def("get_known_object_names", &PlanningSceneInterfaceWrapper::getKnownObjectNamesPython); - PlanningSceneClass.def("get_known_object_names_in_roi", - &PlanningSceneInterfaceWrapper::getKnownObjectNamesInROIPython); - PlanningSceneClass.def("get_object_poses", &PlanningSceneInterfaceWrapper::getObjectPosesPython); - PlanningSceneClass.def("get_objects", &PlanningSceneInterfaceWrapper::getObjectsPython); - PlanningSceneClass.def("get_attached_objects", &PlanningSceneInterfaceWrapper::getAttachedObjectsPython); - PlanningSceneClass.def("apply_planning_scene", &PlanningSceneInterfaceWrapper::applyPlanningScenePython); + bp::class_ planning_scene_class("PlanningSceneInterface", + bp::init>()); + + planning_scene_class.def("get_known_object_names", &PlanningSceneInterfaceWrapper::getKnownObjectNamesPython); + planning_scene_class.def("get_known_object_names_in_roi", + &PlanningSceneInterfaceWrapper::getKnownObjectNamesInROIPython); + planning_scene_class.def("get_object_poses", &PlanningSceneInterfaceWrapper::getObjectPosesPython); + planning_scene_class.def("get_objects", &PlanningSceneInterfaceWrapper::getObjectsPython); + planning_scene_class.def("get_attached_objects", &PlanningSceneInterfaceWrapper::getAttachedObjectsPython); + planning_scene_class.def("apply_planning_scene", &PlanningSceneInterfaceWrapper::applyPlanningScenePython); } } // namespace planning_interface } // namespace moveit diff --git a/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp b/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp index 008ae96df2..dc89737750 100644 --- a/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp +++ b/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp @@ -373,33 +373,33 @@ static void wrap_robot_interface() { using namespace moveit; - bp::class_ RobotClass("RobotInterface", bp::init>()); - - RobotClass.def("get_joint_names", &RobotInterfacePython::getJointNames); - RobotClass.def("get_group_joint_names", &RobotInterfacePython::getGroupJointNames); - RobotClass.def("get_group_default_states", &RobotInterfacePython::getDefaultStateNames); - RobotClass.def("get_group_joint_tips", &RobotInterfacePython::getGroupJointTips); - RobotClass.def("get_group_names", &RobotInterfacePython::getGroupNames); - RobotClass.def("get_link_names", &RobotInterfacePython::getLinkNames); - RobotClass.def("get_group_link_names", &RobotInterfacePython::getGroupLinkNames); - RobotClass.def("get_joint_limits", &RobotInterfacePython::getJointLimits); - RobotClass.def("get_link_pose", &RobotInterfacePython::getLinkPose); - RobotClass.def("get_planning_frame", &RobotInterfacePython::getPlanningFrame); - RobotClass.def("get_current_state", &RobotInterfacePython::getCurrentState); - RobotClass.def("get_current_variable_values", &RobotInterfacePython::getCurrentVariableValues); - RobotClass.def("get_current_joint_values", &RobotInterfacePython::getCurrentJointValues); - RobotClass.def("get_joint_values", &RobotInterfacePython::getJointValues); - RobotClass.def("get_robot_root_link", &RobotInterfacePython::getRobotRootLink); - RobotClass.def("has_group", &RobotInterfacePython::hasGroup); - RobotClass.def("get_robot_name", &RobotInterfacePython::getRobotName); - RobotClass.def("get_robot_markers", &RobotInterfacePython::getRobotMarkers); - RobotClass.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonList); - RobotClass.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersFromMsg); - RobotClass.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonDictList); - RobotClass.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonDict); - RobotClass.def("get_group_markers", &RobotInterfacePython::getRobotMarkersGroup); - RobotClass.def("get_group_markers", &RobotInterfacePython::getRobotMarkersGroupPythonDict); - RobotClass.def("get_parent_group", &RobotInterfacePython::getEndEffectorParentGroup); + bp::class_ robot_class("RobotInterface", bp::init>()); + + robot_class.def("get_joint_names", &RobotInterfacePython::getJointNames); + robot_class.def("get_group_joint_names", &RobotInterfacePython::getGroupJointNames); + robot_class.def("get_group_default_states", &RobotInterfacePython::getDefaultStateNames); + robot_class.def("get_group_joint_tips", &RobotInterfacePython::getGroupJointTips); + robot_class.def("get_group_names", &RobotInterfacePython::getGroupNames); + robot_class.def("get_link_names", &RobotInterfacePython::getLinkNames); + robot_class.def("get_group_link_names", &RobotInterfacePython::getGroupLinkNames); + robot_class.def("get_joint_limits", &RobotInterfacePython::getJointLimits); + robot_class.def("get_link_pose", &RobotInterfacePython::getLinkPose); + robot_class.def("get_planning_frame", &RobotInterfacePython::getPlanningFrame); + robot_class.def("get_current_state", &RobotInterfacePython::getCurrentState); + robot_class.def("get_current_variable_values", &RobotInterfacePython::getCurrentVariableValues); + robot_class.def("get_current_joint_values", &RobotInterfacePython::getCurrentJointValues); + robot_class.def("get_joint_values", &RobotInterfacePython::getJointValues); + robot_class.def("get_robot_root_link", &RobotInterfacePython::getRobotRootLink); + robot_class.def("has_group", &RobotInterfacePython::hasGroup); + robot_class.def("get_robot_name", &RobotInterfacePython::getRobotName); + robot_class.def("get_robot_markers", &RobotInterfacePython::getRobotMarkers); + robot_class.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonList); + robot_class.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersFromMsg); + robot_class.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonDictList); + robot_class.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonDict); + robot_class.def("get_group_markers", &RobotInterfacePython::getRobotMarkersGroup); + robot_class.def("get_group_markers", &RobotInterfacePython::getRobotMarkersGroupPythonDict); + robot_class.def("get_parent_group", &RobotInterfacePython::getEndEffectorParentGroup); } BOOST_PYTHON_MODULE(_moveit_robot_interface) diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp index 3e0326088e..f089025b3e 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -122,9 +122,9 @@ double RobotInteraction::computeLinkMarkerSize(const std::string& link) { Eigen::Vector3d ext = lm->getShapeExtentsAtOrigin(); // drop largest extension and take norm of two remaining - Eigen::MatrixXd::Index maxIndex; - ext.maxCoeff(&maxIndex); - ext[maxIndex] = 0; + Eigen::MatrixXd::Index max_index; + ext.maxCoeff(&max_index); + ext[max_index] = 0; size = 1.01 * ext.norm(); if (size > 0) break; // break, if a non-empty shape was found @@ -165,9 +165,9 @@ double RobotInteraction::computeGroupMarkerSize(const std::string& group) Eigen::Vector3d ext = lm->getShapeExtentsAtOrigin(); // drop largest extension and take norm of two remaining - Eigen::MatrixXd::Index maxIndex; - ext.maxCoeff(&maxIndex); - ext[maxIndex] = 0; + Eigen::MatrixXd::Index max_index; + ext.maxCoeff(&max_index); + ext[max_index] = 0; size = std::max(size, 1.01 * ext.norm()); } 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 728e941b68..cca0d18acf 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 @@ -554,7 +554,7 @@ void MotionPlanningDisplay::displayMetrics(bool start) if (!robot_interaction_ || !planning_scene_monitor_) return; - static const Ogre::Quaternion orientation(1.0, 0.0, 0.0, 0.0); + static const Ogre::Quaternion ORIENTATION(1.0, 0.0, 0.0, 0.0); const std::vector& eef = robot_interaction_->getActiveEndEffectors(); if (eef.empty()) return; @@ -599,9 +599,9 @@ void MotionPlanningDisplay::displayMetrics(bool start) position[2] = t.z() + 0.2; // \todo this should be a param } if (start) - displayTable(text_table, query_start_color_property_->getOgreColor(), position, orientation); + displayTable(text_table, query_start_color_property_->getOgreColor(), position, ORIENTATION); else - displayTable(text_table, query_goal_color_property_->getOgreColor(), position, orientation); + displayTable(text_table, query_goal_color_property_->getOgreColor(), position, ORIENTATION); text_display_for_start_ = start; } } @@ -1359,9 +1359,9 @@ void MotionPlanningDisplay::load(const rviz::Config& config) else { std::string node_name = ros::names::append(getMoveGroupNS(), "move_group"); - ros::NodeHandle nh_(node_name); + ros::NodeHandle nh(node_name); double val; - if (nh_.getParam("default_workspace_bounds", val)) + if (nh.getParam("default_workspace_bounds", val)) { frame_->ui_->wsize_x->setValue(val); frame_->ui_->wsize_y->setValue(val); 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 418abdfbd2..1e234433c5 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 @@ -352,8 +352,8 @@ void MotionPlanningFrame::changePlanningGroupHelper() { move_group_->allowLooking(ui_->allow_looking->isChecked()); move_group_->allowReplanning(ui_->allow_replanning->isChecked()); - bool hasUniqueEndeffector = !move_group_->getEndEffectorLink().empty(); - planning_display_->addMainLoopJob([=]() { ui_->use_cartesian_path->setEnabled(hasUniqueEndeffector); }); + bool has_unique_endeffector = !move_group_->getEndEffectorLink().empty(); + planning_display_->addMainLoopJob([=]() { ui_->use_cartesian_path->setEnabled(has_unique_endeffector); }); moveit_msgs::PlannerInterfaceDescription desc; if (move_group_->getInterfaceDescription(desc)) planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlannersList, this, desc)); @@ -416,12 +416,12 @@ void MotionPlanningFrame::importResource(const std::string& path) // If the object already exists, ask the user whether to overwrite or rename if (planning_display_->getPlanningSceneRO()->getWorld()->hasObject(name)) { - QMessageBox msgBox; - msgBox.setText("There exists another object with the same name."); - msgBox.setInformativeText("Would you like to overwrite it?"); - msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel); - msgBox.setDefaultButton(QMessageBox::No); - int ret = msgBox.exec(); + QMessageBox msg_box; + msg_box.setText("There exists another object with the same name."); + msg_box.setInformativeText("Would you like to overwrite it?"); + msg_box.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel); + msg_box.setDefaultButton(QMessageBox::No); + int ret = msg_box.exec(); switch (ret) { diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp index 982412ae91..aea72abcf2 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp @@ -157,7 +157,7 @@ void MotionPlanningFrame::updateDetectedObjectsList(const std::vector& objects) { ui_->detected_objects_list->setUpdatesEnabled(false); - bool oldState = ui_->detected_objects_list->blockSignals(true); + bool old_state = ui_->detected_objects_list->blockSignals(true); ui_->detected_objects_list->clear(); { for (std::size_t i = 0; i < object_ids.size(); ++i) @@ -171,7 +171,7 @@ void MotionPlanningFrame::updateDetectedObjectsList(const std::vectordetected_objects_list->addItem(item); } } - ui_->detected_objects_list->blockSignals(oldState); + ui_->detected_objects_list->blockSignals(old_state); ui_->detected_objects_list->setUpdatesEnabled(true); if (!object_ids.empty()) ui_->pick_button->setEnabled(true); @@ -227,7 +227,7 @@ void MotionPlanningFrame::updateSupportSurfacesList() ROS_INFO("%d Tables in collision world", (int)support_ids.size()); ui_->support_surfaces_list->setUpdatesEnabled(false); - bool oldState = ui_->support_surfaces_list->blockSignals(true); + bool old_state = ui_->support_surfaces_list->blockSignals(true); ui_->support_surfaces_list->clear(); { for (std::size_t i = 0; i < support_ids.size(); ++i) @@ -241,7 +241,7 @@ void MotionPlanningFrame::updateSupportSurfacesList() ui_->support_surfaces_list->addItem(item); } } - ui_->support_surfaces_list->blockSignals(oldState); + ui_->support_surfaces_list->blockSignals(old_state); ui_->support_surfaces_list->setUpdatesEnabled(true); } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index 4d41584719..09fccb0b76 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -190,29 +190,29 @@ void MotionPlanningFrame::selectedCollisionObjectChanged() QList sel = ui_->collision_objects_list->selectedItems(); if (sel.empty()) { - bool oldState = ui_->object_x->blockSignals(true); + bool old_state = ui_->object_x->blockSignals(true); ui_->object_x->setValue(0.0); - ui_->object_x->blockSignals(oldState); + ui_->object_x->blockSignals(old_state); - oldState = ui_->object_y->blockSignals(true); + old_state = ui_->object_y->blockSignals(true); ui_->object_y->setValue(0.0); - ui_->object_y->blockSignals(oldState); + ui_->object_y->blockSignals(old_state); - oldState = ui_->object_z->blockSignals(true); + old_state = ui_->object_z->blockSignals(true); ui_->object_z->setValue(0.0); - ui_->object_z->blockSignals(oldState); + ui_->object_z->blockSignals(old_state); - oldState = ui_->object_rx->blockSignals(true); + old_state = ui_->object_rx->blockSignals(true); ui_->object_rx->setValue(0.0); - ui_->object_rx->blockSignals(oldState); + ui_->object_rx->blockSignals(old_state); - oldState = ui_->object_ry->blockSignals(true); + old_state = ui_->object_ry->blockSignals(true); ui_->object_ry->setValue(0.0); - ui_->object_ry->blockSignals(oldState); + ui_->object_ry->blockSignals(old_state); - oldState = ui_->object_rz->blockSignals(true); + old_state = ui_->object_rz->blockSignals(true); ui_->object_rz->setValue(0.0); - ui_->object_rz->blockSignals(oldState); + ui_->object_rz->blockSignals(old_state); ui_->object_status->setText(""); scene_marker_.reset(); @@ -240,29 +240,29 @@ void MotionPlanningFrame::selectedCollisionObjectChanged() Eigen::Vector3d xyz = obj_pose.rotation().eulerAngles(0, 1, 2); update_scene_marker = true; // do the marker update outside locked scope to avoid deadlock - bool oldState = ui_->object_x->blockSignals(true); + bool old_state = ui_->object_x->blockSignals(true); ui_->object_x->setValue(obj_pose.translation()[0]); - ui_->object_x->blockSignals(oldState); + ui_->object_x->blockSignals(old_state); - oldState = ui_->object_y->blockSignals(true); + old_state = ui_->object_y->blockSignals(true); ui_->object_y->setValue(obj_pose.translation()[1]); - ui_->object_y->blockSignals(oldState); + ui_->object_y->blockSignals(old_state); - oldState = ui_->object_z->blockSignals(true); + old_state = ui_->object_z->blockSignals(true); ui_->object_z->setValue(obj_pose.translation()[2]); - ui_->object_z->blockSignals(oldState); + ui_->object_z->blockSignals(old_state); - oldState = ui_->object_rx->blockSignals(true); + old_state = ui_->object_rx->blockSignals(true); ui_->object_rx->setValue(xyz[0]); - ui_->object_rx->blockSignals(oldState); + ui_->object_rx->blockSignals(old_state); - oldState = ui_->object_ry->blockSignals(true); + old_state = ui_->object_ry->blockSignals(true); ui_->object_ry->setValue(xyz[1]); - ui_->object_ry->blockSignals(oldState); + ui_->object_ry->blockSignals(old_state); - oldState = ui_->object_rz->blockSignals(true); + old_state = ui_->object_rz->blockSignals(true); ui_->object_rz->setValue(xyz[2]); - ui_->object_rz->blockSignals(oldState); + ui_->object_rz->blockSignals(old_state); } } else @@ -348,33 +348,33 @@ void MotionPlanningFrame::collisionObjectChanged(QListWidgetItem* item) /* Receives feedback from the interactive marker and updates the shape pose in the world accordingly */ void MotionPlanningFrame::imProcessFeedback(visualization_msgs::InteractiveMarkerFeedback& feedback) { - bool oldState = ui_->object_x->blockSignals(true); + bool old_state = ui_->object_x->blockSignals(true); ui_->object_x->setValue(feedback.pose.position.x); - ui_->object_x->blockSignals(oldState); + ui_->object_x->blockSignals(old_state); - oldState = ui_->object_y->blockSignals(true); + old_state = ui_->object_y->blockSignals(true); ui_->object_y->setValue(feedback.pose.position.y); - ui_->object_y->blockSignals(oldState); + ui_->object_y->blockSignals(old_state); - oldState = ui_->object_z->blockSignals(true); + old_state = ui_->object_z->blockSignals(true); ui_->object_z->setValue(feedback.pose.position.z); - ui_->object_z->blockSignals(oldState); + ui_->object_z->blockSignals(old_state); Eigen::Quaterniond q; tf2::fromMsg(feedback.pose.orientation, q); Eigen::Vector3d xyz = q.matrix().eulerAngles(0, 1, 2); - oldState = ui_->object_rx->blockSignals(true); + old_state = ui_->object_rx->blockSignals(true); ui_->object_rx->setValue(xyz[0]); - ui_->object_rx->blockSignals(oldState); + ui_->object_rx->blockSignals(old_state); - oldState = ui_->object_ry->blockSignals(true); + old_state = ui_->object_ry->blockSignals(true); ui_->object_ry->setValue(xyz[1]); - ui_->object_ry->blockSignals(oldState); + ui_->object_ry->blockSignals(old_state); - oldState = ui_->object_rz->blockSignals(true); + old_state = ui_->object_rz->blockSignals(true); ui_->object_rz->setValue(xyz[2]); - ui_->object_rz->blockSignals(oldState); + ui_->object_rz->blockSignals(old_state); updateCollisionObjectPose(false); } @@ -845,7 +845,7 @@ void MotionPlanningFrame::attachDetachCollisionObject(QListWidgetItem* item) void MotionPlanningFrame::populateCollisionObjectsList() { ui_->collision_objects_list->setUpdatesEnabled(false); - bool oldState = ui_->collision_objects_list->blockSignals(true); + bool old_state = ui_->collision_objects_list->blockSignals(true); { QList sel = ui_->collision_objects_list->selectedItems(); @@ -895,7 +895,7 @@ void MotionPlanningFrame::populateCollisionObjectsList() } } - ui_->collision_objects_list->blockSignals(oldState); + ui_->collision_objects_list->blockSignals(old_state); ui_->collision_objects_list->setUpdatesEnabled(true); selectedCollisionObjectChanged(); } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index e595f17119..8fc66c7277 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -385,10 +385,10 @@ void MotionPlanningFrame::populatePlannersList(const moveit_msgs::PlannerInterfa // retrieve default planner config from parameter server const std::string& default_planner_config = move_group_->getDefaultPlannerId(found_group ? group : std::string()); - int defaultIndex = ui_->planning_algorithm_combo_box->findText(QString::fromStdString(default_planner_config)); - if (defaultIndex < 0) - defaultIndex = 0; // 0 is fallback - ui_->planning_algorithm_combo_box->setCurrentIndex(defaultIndex); + int default_index = ui_->planning_algorithm_combo_box->findText(QString::fromStdString(default_planner_config)); + if (default_index < 0) + default_index = 0; // 0 is fallback + ui_->planning_algorithm_combo_box->setCurrentIndex(default_index); } void MotionPlanningFrame::populateConstraintsList() diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp index 94691d524d..1feaf673d1 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp @@ -211,12 +211,12 @@ void MotionPlanningFrame::removeStateButtonClicked() if (robot_state_storage_) { // Warn the user - QMessageBox msgBox; - msgBox.setText("All the selected states will be removed from the database"); - msgBox.setInformativeText("Do you want to continue?"); - msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel); - msgBox.setDefaultButton(QMessageBox::No); - int ret = msgBox.exec(); + QMessageBox msg_box; + msg_box.setText("All the selected states will be removed from the database"); + msg_box.setInformativeText("Do you want to continue?"); + msg_box.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel); + msg_box.setDefaultButton(QMessageBox::No); + int ret = msg_box.exec(); switch (ret) { diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index 086c3b15da..ee5c2f0ae4 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -549,9 +549,9 @@ void PlanningSceneDisplay::onRobotModelLoaded() planning_scene_robot_->update(robot_state::RobotStateConstPtr(rs)); } - bool oldState = scene_name_property_->blockSignals(true); + bool old_state = scene_name_property_->blockSignals(true); scene_name_property_->setStdString(ps->getName()); - scene_name_property_->blockSignals(oldState); + scene_name_property_->blockSignals(old_state); } void PlanningSceneDisplay::sceneMonitorReceivedUpdate( @@ -563,10 +563,10 @@ void PlanningSceneDisplay::sceneMonitorReceivedUpdate( void PlanningSceneDisplay::onSceneMonitorReceivedUpdate( planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) { - bool oldState = scene_name_property_->blockSignals(true); + bool old_state = scene_name_property_->blockSignals(true); getPlanningSceneRW()->getCurrentStateNonConst().update(); scene_name_property_->setStdString(getPlanningSceneRO()->getName()); - scene_name_property_->blockSignals(oldState); + scene_name_property_->blockSignals(old_state); planning_scene_needs_render_ = true; } diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp index 2c2d034db3..c3e687cf0e 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp @@ -363,9 +363,9 @@ void RobotStateDisplay::loadRobotModel() robot_->load(*robot_model_->getURDF()); robot_state_.reset(new robot_state::RobotState(robot_model_)); robot_state_->setToDefaultValues(); - bool oldState = root_link_name_property_->blockSignals(true); + bool old_state = root_link_name_property_->blockSignals(true); root_link_name_property_->setStdString(getRobotModel()->getRootLinkName()); - root_link_name_property_->blockSignals(oldState); + root_link_name_property_->blockSignals(old_state); update_state_ = true; setStatus(rviz::StatusProperty::Ok, "RobotState", "Planning Model Loaded Successfully"); diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp index 850dff2bc6..f427a7315b 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp @@ -157,17 +157,17 @@ void OcTreeRender::setColor(double z_pos, double min_z, double max_z, double col void OcTreeRender::octreeDecoding(const std::shared_ptr& octree, OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode) { - VVPoint pointBuf_; - pointBuf_.resize(octree_depth_); + VVPoint point_buf; + point_buf.resize(octree_depth_); // get dimensions of octree - double minX, minY, minZ, maxX, maxY, maxZ; - octree->getMetricMin(minX, minY, minZ); - octree->getMetricMax(maxX, maxY, maxZ); + double min_x, min_y, min_z, max_x, max_y, max_z; + octree->getMetricMin(min_x, min_y, min_z); + octree->getMetricMax(max_x, max_y, max_z); unsigned int render_mode_mask = static_cast(octree_voxel_rendering); - size_t pointCount = 0; + size_t point_count = 0; { // traverse all leafs in the tree: for (octomap::OcTree::iterator it = octree->begin(octree_depth_), end = octree->end(); it != end; ++it) @@ -178,18 +178,18 @@ void OcTreeRender::octreeDecoding(const std::shared_ptr& if (((int)octree->isNodeOccupied(*it) + 1) & render_mode_mask) { // check if current voxel has neighbors on all sides -> no need to be displayed - bool allNeighborsFound = true; + bool all_neighbors_found = true; octomap::OcTreeKey key; - octomap::OcTreeKey nKey = it.getKey(); + octomap::OcTreeKey n_key = it.getKey(); - for (key[2] = nKey[2] - 1; allNeighborsFound && key[2] <= nKey[2] + 1; ++key[2]) + for (key[2] = n_key[2] - 1; all_neighbors_found && key[2] <= n_key[2] + 1; ++key[2]) { - for (key[1] = nKey[1] - 1; allNeighborsFound && key[1] <= nKey[1] + 1; ++key[1]) + for (key[1] = n_key[1] - 1; all_neighbors_found && key[1] <= n_key[1] + 1; ++key[1]) { - for (key[0] = nKey[0] - 1; allNeighborsFound && key[0] <= nKey[0] + 1; ++key[0]) + for (key[0] = n_key[0] - 1; all_neighbors_found && key[0] <= n_key[0] + 1; ++key[0]) { - if (key != nKey) + if (key != n_key) { octomap::OcTreeNode* node = octree->search(key); @@ -197,34 +197,34 @@ void OcTreeRender::octreeDecoding(const std::shared_ptr& if (!(node && (((int)octree->isNodeOccupied(node)) + 1) & render_mode_mask)) { // we do not have a neighbor => break! - allNeighborsFound = false; + all_neighbors_found = false; } } } } } - display_voxel |= !allNeighborsFound; + display_voxel |= !all_neighbors_found; } if (display_voxel) { - rviz::PointCloud::Point newPoint; + rviz::PointCloud::Point new_point; - newPoint.position.x = it.getX(); - newPoint.position.y = it.getY(); - newPoint.position.z = it.getZ(); + new_point.position.x = it.getX(); + new_point.position.y = it.getY(); + new_point.position.z = it.getZ(); float cell_probability; switch (octree_color_mode) { case OCTOMAP_Z_AXIS_COLOR: - setColor(newPoint.position.z, minZ, maxZ, colorFactor_, &newPoint); + setColor(new_point.position.z, min_z, max_z, colorFactor_, &new_point); break; case OCTOMAP_PROBABLILTY_COLOR: cell_probability = it->getOccupancy(); - newPoint.setColor((1.0f - cell_probability), cell_probability, 0.0); + new_point.setColor((1.0f - cell_probability), cell_probability, 0.0); break; default: break; @@ -232,9 +232,9 @@ void OcTreeRender::octreeDecoding(const std::shared_ptr& // push to point vectors unsigned int depth = it.getDepth(); - pointBuf_[depth - 1].push_back(newPoint); + point_buf[depth - 1].push_back(new_point); - ++pointCount; + ++point_count; } } } @@ -246,8 +246,8 @@ void OcTreeRender::octreeDecoding(const std::shared_ptr& cloud_[i]->clear(); cloud_[i]->setDimensions(size, size, size); - cloud_[i]->addPoints(&pointBuf_[i].front(), pointBuf_[i].size()); - pointBuf_[i].clear(); + cloud_[i]->addPoints(&point_buf[i].front(), point_buf[i].size()); + point_buf[i].clear(); } } } // namespace moveit_rviz_plugin diff --git a/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp b/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp index c5d28de20e..ef4a2a87da 100644 --- a/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp +++ b/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp @@ -63,14 +63,14 @@ void moveit_warehouse::MoveItMessageStorage::filterNames(const std::string& rege } } -static std::unique_ptr dbloader; +static std::unique_ptr DBLOADER; typename warehouse_ros::DatabaseConnection::Ptr moveit_warehouse::loadDatabase() { - if (!dbloader) + if (!DBLOADER) { - dbloader.reset(new warehouse_ros::DatabaseLoader()); + DBLOADER.reset(new warehouse_ros::DatabaseLoader()); } - return dbloader->loadDatabase(); + return DBLOADER->loadDatabase(); // return typename warehouse_ros::DatabaseConnection::Ptr(new warehouse_ros_mongo::MongoDatabaseConnection()); } diff --git a/moveit_ros/warehouse/warehouse/src/save_as_text.cpp b/moveit_ros/warehouse/warehouse/src/save_as_text.cpp index 76d302cf15..7fd374dd98 100644 --- a/moveit_ros/warehouse/warehouse/src/save_as_text.cpp +++ b/moveit_ros/warehouse/warehouse/src/save_as_text.cpp @@ -123,52 +123,52 @@ int main(int argc, char** argv) psm.getPlanningScene()->saveGeometryToStream(fout); fout.close(); - std::vector robotStateNames; + std::vector robot_state_names; robot_model::RobotModelConstPtr km = psm.getRobotModel(); // Get start states for scene std::stringstream rsregex; rsregex << ".*" << scene_names[i] << ".*"; - rss.getKnownRobotStates(rsregex.str(), robotStateNames); + rss.getKnownRobotStates(rsregex.str(), robot_state_names); // Get goal constraints for scene - std::vector constraintNames; + std::vector constraint_names; std::stringstream csregex; csregex << ".*" << scene_names[i] << ".*"; - cs.getKnownConstraints(csregex.str(), constraintNames); + cs.getKnownConstraints(csregex.str(), constraint_names); - if (!(robotStateNames.empty() && constraintNames.empty())) + if (!(robot_state_names.empty() && constraint_names.empty())) { std::ofstream qfout((scene_names[i] + ".queries").c_str()); qfout << scene_names[i] << std::endl; - if (!robotStateNames.empty()) + if (!robot_state_names.empty()) { qfout << "start" << std::endl; - qfout << robotStateNames.size() << std::endl; - for (std::size_t k = 0; k < robotStateNames.size(); ++k) + qfout << robot_state_names.size() << std::endl; + for (std::size_t k = 0; k < robot_state_names.size(); ++k) { - ROS_INFO("Saving start state %s for scene %s", robotStateNames[k].c_str(), scene_names[i].c_str()); - qfout << robotStateNames[k] << std::endl; - moveit_warehouse::RobotStateWithMetadata robotState; - rss.getRobotState(robotState, robotStateNames[k]); + ROS_INFO("Saving start state %s for scene %s", robot_state_names[k].c_str(), scene_names[i].c_str()); + qfout << robot_state_names[k] << std::endl; + moveit_warehouse::RobotStateWithMetadata robot_state; + rss.getRobotState(robot_state, robot_state_names[k]); robot_state::RobotState ks(km); - robot_state::robotStateMsgToRobotState(*robotState, ks, false); + robot_state::robotStateMsgToRobotState(*robot_state, ks, false); ks.printStateInfo(qfout); qfout << "." << std::endl; } } - if (!constraintNames.empty()) + if (!constraint_names.empty()) { qfout << "goal" << std::endl; - qfout << constraintNames.size() << std::endl; - for (std::size_t k = 0; k < constraintNames.size(); ++k) + qfout << constraint_names.size() << std::endl; + for (std::size_t k = 0; k < constraint_names.size(); ++k) { - ROS_INFO("Saving goal %s for scene %s", constraintNames[k].c_str(), scene_names[i].c_str()); + ROS_INFO("Saving goal %s for scene %s", constraint_names[k].c_str(), scene_names[i].c_str()); qfout << "link_constraint" << std::endl; - qfout << constraintNames[k] << std::endl; + qfout << constraint_names[k] << std::endl; moveit_warehouse::ConstraintsWithMetadata constraints; - cs.getConstraints(constraints, constraintNames[k]); + cs.getConstraints(constraints, constraint_names[k]); LinkConstraintMap lcmap; collectLinkConstraints(*constraints, lcmap); diff --git a/moveit_setup_assistant/src/setup_assistant_main.cpp b/moveit_setup_assistant/src/setup_assistant_main.cpp index 58af9b1c50..a514696622 100644 --- a/moveit_setup_assistant/src/setup_assistant_main.cpp +++ b/moveit_setup_assistant/src/setup_assistant_main.cpp @@ -89,7 +89,7 @@ int main(int argc, char** argv) ros::NodeHandle nh; // Create Qt Application - QApplication qtApp(argc, argv); + QApplication qt_app(argc, argv); // numeric values should always be POSIX setlocale(LC_NUMERIC, "C"); @@ -104,7 +104,7 @@ int main(int argc, char** argv) signal(SIGINT, siginthandler); // Wait here until Qt App is finished - const int result = qtApp.exec(); + const int result = qt_app.exec(); // Shutdown ROS ros::shutdown(); diff --git a/moveit_setup_assistant/src/tools/collision_linear_model.cpp b/moveit_setup_assistant/src/tools/collision_linear_model.cpp index 39510b0235..ea737dc63a 100644 --- a/moveit_setup_assistant/src/tools/collision_linear_model.cpp +++ b/moveit_setup_assistant/src/tools/collision_linear_model.cpp @@ -101,45 +101,45 @@ QModelIndex CollisionLinearModel::parent(const QModelIndex& child) const QVariant CollisionLinearModel::data(const QModelIndex& index, int role) const { - QModelIndex srcIndex = this->mapToSource(index); + QModelIndex src_index = this->mapToSource(index); switch (index.column()) { case 0: // link name 1 if (role != Qt::DisplayRole) return QVariant(); else - return this->sourceModel()->headerData(srcIndex.row(), Qt::Horizontal, Qt::DisplayRole); + return this->sourceModel()->headerData(src_index.row(), Qt::Horizontal, Qt::DisplayRole); case 1: // link name 2 if (role != Qt::DisplayRole) return QVariant(); - return this->sourceModel()->headerData(srcIndex.column(), Qt::Vertical, Qt::DisplayRole); + return this->sourceModel()->headerData(src_index.column(), Qt::Vertical, Qt::DisplayRole); case 2: // checkbox if (role != Qt::CheckStateRole) return QVariant(); else - return this->sourceModel()->data(srcIndex, Qt::CheckStateRole); + return this->sourceModel()->data(src_index, Qt::CheckStateRole); case 3: // reason if (role != Qt::DisplayRole) return QVariant(); else - return this->sourceModel()->data(srcIndex, Qt::ToolTipRole); + return this->sourceModel()->data(src_index, Qt::ToolTipRole); } return QVariant(); } DisabledReason CollisionLinearModel::reason(int row) const { - QModelIndex srcIndex = this->mapToSource(index(row, 0)); - return qobject_cast(sourceModel())->reason(srcIndex); + QModelIndex src_index = this->mapToSource(index(row, 0)); + return qobject_cast(sourceModel())->reason(src_index); } bool CollisionLinearModel::setData(const QModelIndex& index, const QVariant& value, int role) { - QModelIndex srcIndex = this->mapToSource(index); + QModelIndex src_index = this->mapToSource(index); if (role == Qt::CheckStateRole) { - sourceModel()->setData(srcIndex, value, role); + sourceModel()->setData(src_index, value, role); int r = index.row(); Q_EMIT dataChanged(this->index(r, 2), this->index(r, 3)); // reason changed too return true; diff --git a/moveit_setup_assistant/src/tools/collision_matrix_model.cpp b/moveit_setup_assistant/src/tools/collision_matrix_model.cpp index 275b78951b..ea38166312 100644 --- a/moveit_setup_assistant/src/tools/collision_matrix_model.cpp +++ b/moveit_setup_assistant/src/tools/collision_matrix_model.cpp @@ -47,7 +47,7 @@ using namespace moveit_setup_assistant; /// Boost mapping of reasons for disabling a link pair to strings -static const boost::unordered_map longReasonsToString = +static const boost::unordered_map LONG_REASONS_TO_STRING = boost::assign::map_list_of // clang-format off ( moveit_setup_assistant::NEVER, "Never in Collision" ) ( moveit_setup_assistant::DEFAULT, "Collision by Default" ) @@ -57,7 +57,7 @@ static const boost::unordered_map longReasonsToBrush = +static const boost::unordered_map LONG_REASONS_TO_BRUSH = boost::assign::map_list_of // clang-format off ( moveit_setup_assistant::NEVER, QBrush(QColor("lightgreen")) ) ( moveit_setup_assistant::DEFAULT, QBrush(QColor("lightpink")) ) @@ -116,9 +116,9 @@ QVariant CollisionMatrixModel::data(const QModelIndex& index, int role) const case Qt::CheckStateRole: return item->second.disable_check ? Qt::Checked : Qt::Unchecked; case Qt::ToolTipRole: - return longReasonsToString.at(item->second.reason); + return LONG_REASONS_TO_STRING.at(item->second.reason); case Qt::BackgroundRole: - return longReasonsToBrush.at(item->second.reason); + return LONG_REASONS_TO_BRUSH.at(item->second.reason); } return QVariant(); } diff --git a/moveit_setup_assistant/src/tools/compute_default_collisions.cpp b/moveit_setup_assistant/src/tools/compute_default_collisions.cpp index ba70e341bb..06b6cdcb90 100644 --- a/moveit_setup_assistant/src/tools/compute_default_collisions.cpp +++ b/moveit_setup_assistant/src/tools/compute_default_collisions.cpp @@ -49,11 +49,12 @@ namespace moveit_setup_assistant // ****************************************************************************************** // Boost mapping of reasons for disabling a link pair to strings -const boost::unordered_map reasonsToString = boost::assign::map_list_of(NEVER, "Never")( +const boost::unordered_map REASONS_TO_STRING = boost::assign::map_list_of(NEVER, "Never")( DEFAULT, "Default")(ADJACENT, "Adjacent")(ALWAYS, "Always")(USER, "User")(NOT_DISABLED, "Not Disabled"); -const boost::unordered_map reasonsFromString = boost::assign::map_list_of("Never", NEVER)( - "Default", DEFAULT)("Adjacent", ADJACENT)("Always", ALWAYS)("User", USER)("Not Disabled", NOT_DISABLED); +const boost::unordered_map REASONS_FROM_STRING = + boost::assign::map_list_of("Never", NEVER)("Default", DEFAULT)("Adjacent", ADJACENT)("Always", ALWAYS)( + "User", USER)("Not Disabled", NOT_DISABLED); // Unique set of pairs of links in string-based form typedef std::set > StringPairSet; @@ -284,7 +285,7 @@ LinkPairMap computeDefaultCollisions(const planning_scene::PlanningSceneConstPtr bool setLinkPair(const std::string& linkA, const std::string& linkB, const DisabledReason reason, LinkPairMap& link_pairs) { - bool isUnique = false; // determine if this link pair had already existsed in the link_pairs datastructure + bool is_unique = false; // determine if this link pair had already existsed in the link_pairs datastructure // Determine order of the 2 links in the pair std::pair link_pair; @@ -305,14 +306,14 @@ bool setLinkPair(const std::string& linkA, const std::string& linkB, const Disab // Check if link pair was already disabled. It also creates the entry if none existed if (!link_pairs[link_pair].disable_check) // it was not previously disabled { - isUnique = true; + is_unique = true; link_pair_ptr->reason = reason; // only change the reason if the pair was previously enabled } // Only disable collision checking if there is a reason to disable it. This func is also used for initializing pairs link_pair_ptr->disable_check = (reason != NOT_DISABLED); - return isUnique; + return is_unique; } // ****************************************************************************************** @@ -478,19 +479,19 @@ unsigned int disableAlwaysInCollision(planning_scene::PlanningScene& scene, Link double min_collision_faction) { // Trial count variables - static const unsigned int small_trial_count = 200; - static const unsigned int small_trial_limit = (unsigned int)((double)small_trial_count * min_collision_faction); + static const unsigned int SMALL_TRIAL_COUNT = 200; + static const unsigned int SMALL_TRIAL_LIMIT = (unsigned int)((double)SMALL_TRIAL_COUNT * min_collision_faction); bool done = false; unsigned int num_disabled = 0; while (!done) { - // DO 'small_trial_count' COLLISION CHECKS AND RECORD STATISTICS --------------------------------------- + // DO 'SMALL_TRIAL_COUNT' COLLISION CHECKS AND RECORD STATISTICS --------------------------------------- std::map, unsigned int> collision_count; // Do a large number of tests - for (unsigned int i = 0; i < small_trial_count; ++i) + for (unsigned int i = 0; i < SMALL_TRIAL_COUNT; ++i) { // Check for collisions collision_detection::CollisionResult res; @@ -524,7 +525,7 @@ unsigned int disableAlwaysInCollision(planning_scene::PlanningScene& scene, Link it != collision_count.end(); ++it) { // Disable these two links permanently - if (it->second > small_trial_limit) + if (it->second > SMALL_TRIAL_LIMIT) { num_disabled += setLinkPair(it->first.first, it->first.second, ALWAYS, link_pairs); @@ -654,7 +655,7 @@ void disableNeverInCollisionThread(ThreadComputation tc) // ****************************************************************************************** const std::string disabledReasonToString(DisabledReason reason) { - return reasonsToString.at(reason); + return REASONS_TO_STRING.at(reason); } // ****************************************************************************************** @@ -665,7 +666,7 @@ DisabledReason disabledReasonFromString(const std::string& reason) DisabledReason r; try { - r = reasonsFromString.at(reason); + r = REASONS_FROM_STRING.at(reason); } catch (std::out_of_range) { diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index 32e69ffa78..99ff556294 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -445,7 +445,7 @@ std::string MoveItConfigData::getGazeboCompatibleURDF() TiXmlElement transmission("transmission"); TiXmlElement type("type"); TiXmlElement joint("joint"); - TiXmlElement hardwareInterface("hardwareInterface"); + TiXmlElement hardware_interface("hardwareInterface"); TiXmlElement actuator("actuator"); TiXmlElement mechanical_reduction("mechanicalReduction"); @@ -456,12 +456,12 @@ std::string MoveItConfigData::getGazeboCompatibleURDF() type.InsertEndChild(TiXmlText("transmission_interface/SimpleTransmission")); transmission.InsertEndChild(type); - hardwareInterface.InsertEndChild(TiXmlText(getJointHardwareInterface(joint_name).c_str())); - joint.InsertEndChild(hardwareInterface); + hardware_interface.InsertEndChild(TiXmlText(getJointHardwareInterface(joint_name).c_str())); + joint.InsertEndChild(hardware_interface); transmission.InsertEndChild(joint); mechanical_reduction.InsertEndChild(TiXmlText("1")); - actuator.InsertEndChild(hardwareInterface); + actuator.InsertEndChild(hardware_interface); actuator.InsertEndChild(mechanical_reduction); transmission.InsertEndChild(actuator); @@ -550,183 +550,183 @@ std::vector MoveItConfigData::getOMPLPlanners() { std::vector planner_des; - OMPLPlannerDescription SBL("SBL", "geometric"); - SBL.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()"); - planner_des.push_back(SBL); + OMPLPlannerDescription sbl("SBL", "geometric"); + sbl.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()"); + planner_des.push_back(sbl); - OMPLPlannerDescription EST("EST", "geometric"); - EST.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()"); - EST.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); - planner_des.push_back(EST); + OMPLPlannerDescription est("EST", "geometric"); + est.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()"); + est.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); + planner_des.push_back(est); - OMPLPlannerDescription LBKPIECE("LBKPIECE", "geometric"); - LBKPIECE.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + OMPLPlannerDescription lbkpiece("LBKPIECE", "geometric"); + lbkpiece.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " "setup()"); - LBKPIECE.addParameter("border_fraction", "0.9", "Fraction of time focused on boarder default: 0.9"); - LBKPIECE.addParameter("min_valid_path_fraction", "0.5", "Accept partially valid moves above fraction. default: 0.5"); - planner_des.push_back(LBKPIECE); + lbkpiece.addParameter("border_fraction", "0.9", "Fraction of time focused on boarder default: 0.9"); + lbkpiece.addParameter("min_valid_path_fraction", "0.5", "Accept partially valid moves above fraction. default: 0.5"); + planner_des.push_back(lbkpiece); - OMPLPlannerDescription BKPIECE("BKPIECE", "geometric"); - BKPIECE.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + OMPLPlannerDescription bkpiece("BKPIECE", "geometric"); + bkpiece.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " "setup()"); - BKPIECE.addParameter("border_fraction", "0.9", "Fraction of time focused on boarder default: 0.9"); - BKPIECE.addParameter("failed_expansion_score_factor", "0.5", "When extending motion fails, scale score by factor. " + bkpiece.addParameter("border_fraction", "0.9", "Fraction of time focused on boarder default: 0.9"); + bkpiece.addParameter("failed_expansion_score_factor", "0.5", "When extending motion fails, scale score by factor. " "default: 0.5"); - BKPIECE.addParameter("min_valid_path_fraction", "0.5", "Accept partially valid moves above fraction. default: 0.5"); - planner_des.push_back(BKPIECE); + bkpiece.addParameter("min_valid_path_fraction", "0.5", "Accept partially valid moves above fraction. default: 0.5"); + planner_des.push_back(bkpiece); - OMPLPlannerDescription KPIECE("KPIECE", "geometric"); - KPIECE.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + OMPLPlannerDescription kpiece("KPIECE", "geometric"); + kpiece.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " "setup()"); - KPIECE.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); - KPIECE.addParameter("border_fraction", "0.9", "Fraction of time focused on boarder default: 0.9 (0.0,1.]"); - KPIECE.addParameter("failed_expansion_score_factor", "0.5", "When extending motion fails, scale score by factor. " + kpiece.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); + kpiece.addParameter("border_fraction", "0.9", "Fraction of time focused on boarder default: 0.9 (0.0,1.]"); + kpiece.addParameter("failed_expansion_score_factor", "0.5", "When extending motion fails, scale score by factor. " "default: 0.5"); - KPIECE.addParameter("min_valid_path_fraction", "0.5", "Accept partially valid moves above fraction. default: 0.5"); - planner_des.push_back(KPIECE); + kpiece.addParameter("min_valid_path_fraction", "0.5", "Accept partially valid moves above fraction. default: 0.5"); + planner_des.push_back(kpiece); - OMPLPlannerDescription RRT("RRT", "geometric"); - RRT.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()"); - RRT.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability? default: 0.05"); - planner_des.push_back(RRT); + OMPLPlannerDescription rrt("RRT", "geometric"); + rrt.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()"); + rrt.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability? default: 0.05"); + planner_des.push_back(rrt); - OMPLPlannerDescription RRTConnect("RRTConnect", "geometric"); - RRTConnect.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); - planner_des.push_back(RRTConnect); + OMPLPlannerDescription rrt_connect("RRTConnect", "geometric"); + rrt_connect.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); + planner_des.push_back(rrt_connect); - OMPLPlannerDescription RRTstar("RRTstar", "geometric"); - RRTstar.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); - RRTstar.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability? default: 0.05"); - RRTstar.addParameter("delay_collision_checking", "1", "Stop collision checking as soon as C-free parent found. " - "default 1"); - planner_des.push_back(RRTstar); - - OMPLPlannerDescription TRRT("TRRT", "geometric"); - TRRT.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()"); - TRRT.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability? default: 0.05"); - TRRT.addParameter("max_states_failed", "10", "when to start increasing temp. default: 10"); - TRRT.addParameter("temp_change_factor", "2.0", "how much to increase or decrease temp. default: 2.0"); - TRRT.addParameter("min_temperature", "10e-10", "lower limit of temp change. default: 10e-10"); - TRRT.addParameter("init_temperature", "10e-6", "initial temperature. default: 10e-6"); - TRRT.addParameter("frountier_threshold", "0.0", "dist new state to nearest neighbor to disqualify as frontier. " + OMPLPlannerDescription rr_tstar("RRTstar", "geometric"); + rr_tstar.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); + rr_tstar.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability? default: 0.05"); + rr_tstar.addParameter("delay_collision_checking", "1", "Stop collision checking as soon as C-free parent found. " + "default 1"); + planner_des.push_back(rr_tstar); + + OMPLPlannerDescription trrt("TRRT", "geometric"); + trrt.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()"); + trrt.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability? default: 0.05"); + trrt.addParameter("max_states_failed", "10", "when to start increasing temp. default: 10"); + trrt.addParameter("temp_change_factor", "2.0", "how much to increase or decrease temp. default: 2.0"); + trrt.addParameter("min_temperature", "10e-10", "lower limit of temp change. default: 10e-10"); + trrt.addParameter("init_temperature", "10e-6", "initial temperature. default: 10e-6"); + trrt.addParameter("frountier_threshold", "0.0", "dist new state to nearest neighbor to disqualify as frontier. " "default: 0.0 set in setup()"); - TRRT.addParameter("frountierNodeRatio", "0.1", "1/10, or 1 nonfrontier for every 10 frontier. default: 0.1"); - TRRT.addParameter("k_constant", "0.0", "value used to normalize expresssion. default: 0.0 set in setup()"); - planner_des.push_back(TRRT); - - OMPLPlannerDescription PRM("PRM", "geometric"); - PRM.addParameter("max_nearest_neighbors", "10", "use k nearest neighbors. default: 10"); - planner_des.push_back(PRM); - - OMPLPlannerDescription PRMstar("PRMstar", "geometric"); // no declares in code - planner_des.push_back(PRMstar); - - OMPLPlannerDescription FMT("FMT", "geometric"); - FMT.addParameter("num_samples", "1000", "number of states that the planner should sample. default: 1000"); - FMT.addParameter("radius_multiplier", "1.1", "multiplier used for the nearest neighbors search radius. default: 1.1"); - FMT.addParameter("nearest_k", "1", "use Knearest strategy. default: 1"); - FMT.addParameter("cache_cc", "1", "use collision checking cache. default: 1"); - FMT.addParameter("heuristics", "0", "activate cost to go heuristics. default: 0"); - FMT.addParameter("extended_fmt", "1", "activate the extended FMT*: adding new samples if planner does not finish " + trrt.addParameter("frountierNodeRatio", "0.1", "1/10, or 1 nonfrontier for every 10 frontier. default: 0.1"); + trrt.addParameter("k_constant", "0.0", "value used to normalize expresssion. default: 0.0 set in setup()"); + planner_des.push_back(trrt); + + OMPLPlannerDescription prm("PRM", "geometric"); + prm.addParameter("max_nearest_neighbors", "10", "use k nearest neighbors. default: 10"); + planner_des.push_back(prm); + + OMPLPlannerDescription pr_mstar("PRMstar", "geometric"); // no declares in code + planner_des.push_back(pr_mstar); + + OMPLPlannerDescription fmt("FMT", "geometric"); + fmt.addParameter("num_samples", "1000", "number of states that the planner should sample. default: 1000"); + fmt.addParameter("radius_multiplier", "1.1", "multiplier used for the nearest neighbors search radius. default: 1.1"); + fmt.addParameter("nearest_k", "1", "use Knearest strategy. default: 1"); + fmt.addParameter("cache_cc", "1", "use collision checking cache. default: 1"); + fmt.addParameter("heuristics", "0", "activate cost to go heuristics. default: 0"); + fmt.addParameter("extended_fmt", "1", "activate the extended FMT*: adding new samples if planner does not finish " "successfully. default: 1"); - planner_des.push_back(FMT); + planner_des.push_back(fmt); - OMPLPlannerDescription BFMT("BFMT", "geometric"); - BFMT.addParameter("num_samples", "1000", "number of states that the planner should sample. default: 1000"); - BFMT.addParameter("radius_multiplier", "1.0", "multiplier used for the nearest neighbors search radius. default: " + OMPLPlannerDescription bfmt("BFMT", "geometric"); + bfmt.addParameter("num_samples", "1000", "number of states that the planner should sample. default: 1000"); + bfmt.addParameter("radius_multiplier", "1.0", "multiplier used for the nearest neighbors search radius. default: " "1.0"); - BFMT.addParameter("nearest_k", "1", "use the Knearest strategy. default: 1"); - BFMT.addParameter("balanced", "0", "exploration strategy: balanced true expands one tree every iteration. False will " + bfmt.addParameter("nearest_k", "1", "use the Knearest strategy. default: 1"); + bfmt.addParameter("balanced", "0", "exploration strategy: balanced true expands one tree every iteration. False will " "select the tree with lowest maximum cost to go. default: 1"); - BFMT.addParameter("optimality", "1", "termination strategy: optimality true finishes when the best possible path is " + bfmt.addParameter("optimality", "1", "termination strategy: optimality true finishes when the best possible path is " "found. Otherwise, the algorithm will finish when the first feasible path is " "found. default: 1"); - BFMT.addParameter("heuristics", "1", "activates cost to go heuristics. default: 1"); - BFMT.addParameter("cache_cc", "1", "use the collision checking cache. default: 1"); - BFMT.addParameter("extended_fmt", "1", "Activates the extended FMT*: adding new samples if planner does not finish " + bfmt.addParameter("heuristics", "1", "activates cost to go heuristics. default: 1"); + bfmt.addParameter("cache_cc", "1", "use the collision checking cache. default: 1"); + bfmt.addParameter("extended_fmt", "1", "Activates the extended FMT*: adding new samples if planner does not finish " "successfully. default: 1"); - planner_des.push_back(BFMT); + planner_des.push_back(bfmt); - OMPLPlannerDescription PDST("PDST", "geometric"); - RRT.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability? default: 0.05"); - planner_des.push_back(PDST); + OMPLPlannerDescription pdst("PDST", "geometric"); + rrt.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability? default: 0.05"); + planner_des.push_back(pdst); - OMPLPlannerDescription STRIDE("STRIDE", "geometric"); - STRIDE.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + OMPLPlannerDescription stride("STRIDE", "geometric"); + stride.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " "setup()"); - STRIDE.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); - STRIDE.addParameter("use_projected_distance", "0", "whether nearest neighbors are computed based on distances in a " + stride.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); + stride.addParameter("use_projected_distance", "0", "whether nearest neighbors are computed based on distances in a " "projection of the state rather distances in the state space " "itself. default: 0"); - STRIDE.addParameter("degree", "16", "desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). " + stride.addParameter("degree", "16", "desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). " "default: 16"); - STRIDE.addParameter("max_degree", "18", "max degree of a node in the GNAT. default: 12"); - STRIDE.addParameter("min_degree", "12", "min degree of a node in the GNAT. default: 12"); - STRIDE.addParameter("max_pts_per_leaf", "6", "max points per leaf in the GNAT. default: 6"); - STRIDE.addParameter("estimated_dimension", "0.0", "estimated dimension of the free space. default: 0.0"); - STRIDE.addParameter("min_valid_path_fraction", "0.2", "Accept partially valid moves above fraction. default: 0.2"); - planner_des.push_back(STRIDE); - - OMPLPlannerDescription BiTRRT("BiTRRT", "geometric"); - BiTRRT.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); - BiTRRT.addParameter("temp_change_factor", "0.1", "how much to increase or decrease temp. default: 0.1"); - BiTRRT.addParameter("init_temperature", "100", "initial temperature. default: 100"); - BiTRRT.addParameter("frountier_threshold", "0.0", "dist new state to nearest neighbor to disqualify as frontier. " - "default: 0.0 set in setup()"); - BiTRRT.addParameter("frountier_node_ratio", "0.1", "1/10, or 1 nonfrontier for every 10 frontier. default: 0.1"); - BiTRRT.addParameter("cost_threshold", "1e300", "the cost threshold. Any motion cost that is not better will not be " - "expanded. default: inf"); - planner_des.push_back(BiTRRT); - - OMPLPlannerDescription LBTRRT("LBTRRT", "geometric"); - LBTRRT.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + stride.addParameter("max_degree", "18", "max degree of a node in the GNAT. default: 12"); + stride.addParameter("min_degree", "12", "min degree of a node in the GNAT. default: 12"); + stride.addParameter("max_pts_per_leaf", "6", "max points per leaf in the GNAT. default: 6"); + stride.addParameter("estimated_dimension", "0.0", "estimated dimension of the free space. default: 0.0"); + stride.addParameter("min_valid_path_fraction", "0.2", "Accept partially valid moves above fraction. default: 0.2"); + planner_des.push_back(stride); + + OMPLPlannerDescription bi_trrt("BiTRRT", "geometric"); + bi_trrt.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); + bi_trrt.addParameter("temp_change_factor", "0.1", "how much to increase or decrease temp. default: 0.1"); + bi_trrt.addParameter("init_temperature", "100", "initial temperature. default: 100"); + bi_trrt.addParameter("frountier_threshold", "0.0", "dist new state to nearest neighbor to disqualify as frontier. " + "default: 0.0 set in setup()"); + bi_trrt.addParameter("frountier_node_ratio", "0.1", "1/10, or 1 nonfrontier for every 10 frontier. default: 0.1"); + bi_trrt.addParameter("cost_threshold", "1e300", "the cost threshold. Any motion cost that is not better will not be " + "expanded. default: inf"); + planner_des.push_back(bi_trrt); + + OMPLPlannerDescription lbtrrt("LBTRRT", "geometric"); + lbtrrt.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " "setup()"); - LBTRRT.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); - LBTRRT.addParameter("epsilon", "0.4", "optimality approximation factor. default: 0.4"); - planner_des.push_back(LBTRRT); + lbtrrt.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); + lbtrrt.addParameter("epsilon", "0.4", "optimality approximation factor. default: 0.4"); + planner_des.push_back(lbtrrt); - OMPLPlannerDescription BiEST("BiEST", "geometric"); - BiEST.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); - planner_des.push_back(BiEST); + OMPLPlannerDescription bi_est("BiEST", "geometric"); + bi_est.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); + planner_des.push_back(bi_est); - OMPLPlannerDescription ProjEST("ProjEST", "geometric"); - ProjEST.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); - ProjEST.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); - planner_des.push_back(ProjEST); + OMPLPlannerDescription proj_est("ProjEST", "geometric"); + proj_est.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); + proj_est.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); + planner_des.push_back(proj_est); - OMPLPlannerDescription LazyPRM("LazyPRM", "geometric"); - LazyPRM.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); - planner_des.push_back(LazyPRM); + OMPLPlannerDescription lazy_prm("LazyPRM", "geometric"); + lazy_prm.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); + planner_des.push_back(lazy_prm); - OMPLPlannerDescription LazyPRMstar("LazyPRMstar", "geometric"); // no declares in code - planner_des.push_back(LazyPRMstar); + OMPLPlannerDescription lazy_pr_mstar("LazyPRMstar", "geometric"); // no declares in code + planner_des.push_back(lazy_pr_mstar); - OMPLPlannerDescription SPARS("SPARS", "geometric"); - SPARS.addParameter("stretch_factor", "3.0", "roadmap spanner stretch factor. multiplicative upper bound on path " + OMPLPlannerDescription spars("SPARS", "geometric"); + spars.addParameter("stretch_factor", "3.0", "roadmap spanner stretch factor. multiplicative upper bound on path " "quality. It does not make sense to make this parameter more than 3. " "default: 3.0"); - SPARS.addParameter("sparse_delta_fraction", "0.25", "delta fraction for connection distance. This value represents " + spars.addParameter("sparse_delta_fraction", "0.25", "delta fraction for connection distance. This value represents " "the visibility range of sparse samples. default: 0.25"); - SPARS.addParameter("dense_delta_fraction", "0.001", "delta fraction for interface detection. default: 0.001"); - SPARS.addParameter("max_failures", "1000", "maximum consecutive failure limit. default: 1000"); - planner_des.push_back(SPARS); - - OMPLPlannerDescription SPARStwo("SPARStwo", "geometric"); - SPARStwo.addParameter("stretch_factor", "3.0", "roadmap spanner stretch factor. multiplicative upper bound on path " - "quality. It does not make sense to make this parameter more than 3. " - "default: 3.0"); - SPARStwo.addParameter("sparse_delta_fraction", "0.25", - "delta fraction for connection distance. This value represents " - "the visibility range of sparse samples. default: 0.25"); - SPARStwo.addParameter("dense_delta_fraction", "0.001", "delta fraction for interface detection. default: 0.001"); - SPARStwo.addParameter("max_failures", "5000", "maximum consecutive failure limit. default: 5000"); - planner_des.push_back(SPARStwo); + spars.addParameter("dense_delta_fraction", "0.001", "delta fraction for interface detection. default: 0.001"); + spars.addParameter("max_failures", "1000", "maximum consecutive failure limit. default: 1000"); + planner_des.push_back(spars); + + OMPLPlannerDescription spar_stwo("SPARStwo", "geometric"); + spar_stwo.addParameter("stretch_factor", "3.0", "roadmap spanner stretch factor. multiplicative upper bound on path " + "quality. It does not make sense to make this parameter more than 3. " + "default: 3.0"); + spar_stwo.addParameter("sparse_delta_fraction", "0.25", + "delta fraction for connection distance. This value represents " + "the visibility range of sparse samples. default: 0.25"); + spar_stwo.addParameter("dense_delta_fraction", "0.001", "delta fraction for interface detection. default: 0.001"); + spar_stwo.addParameter("max_failures", "5000", "maximum consecutive failure limit. default: 5000"); + planner_des.push_back(spar_stwo); return planner_des; } diff --git a/moveit_setup_assistant/src/widgets/author_information_widget.cpp b/moveit_setup_assistant/src/widgets/author_information_widget.cpp index e97734368b..fc9dc50727 100644 --- a/moveit_setup_assistant/src/widgets/author_information_widget.cpp +++ b/moveit_setup_assistant/src/widgets/author_information_widget.cpp @@ -70,7 +70,7 @@ AuthorInformationWidget::AuthorInformationWidget(QWidget* parent, const MoveItCo layout->addWidget(name_title); name_edit_ = new QLineEdit(this); - connect(name_edit_, SIGNAL(editingFinished()), this, SLOT(edited_name())); + connect(name_edit_, SIGNAL(editingFinished()), this, SLOT(editedName())); layout->addWidget(name_edit_); QLabel* email_title = new QLabel(this); @@ -78,7 +78,7 @@ AuthorInformationWidget::AuthorInformationWidget(QWidget* parent, const MoveItCo layout->addWidget(email_title); email_edit_ = new QLineEdit(this); - connect(email_edit_, SIGNAL(editingFinished()), this, SLOT(edited_email())); + connect(email_edit_, SIGNAL(editingFinished()), this, SLOT(editedEmail())); layout->addWidget(email_edit_); // Finish Layout -------------------------------------------------- @@ -95,13 +95,13 @@ void AuthorInformationWidget::focusGiven() this->email_edit_->setText(QString::fromStdString(config_data_->author_email_)); } -void AuthorInformationWidget::edited_name() +void AuthorInformationWidget::editedName() { config_data_->author_name_ = this->name_edit_->text().toStdString(); config_data_->changes |= MoveItConfigData::AUTHOR_INFO; } -void AuthorInformationWidget::edited_email() +void AuthorInformationWidget::editedEmail() { config_data_->author_email_ = this->email_edit_->text().toStdString(); config_data_->changes |= MoveItConfigData::AUTHOR_INFO; diff --git a/moveit_setup_assistant/src/widgets/author_information_widget.h b/moveit_setup_assistant/src/widgets/author_information_widget.h index d506de103c..15ed0036ec 100644 --- a/moveit_setup_assistant/src/widgets/author_information_widget.h +++ b/moveit_setup_assistant/src/widgets/author_information_widget.h @@ -77,8 +77,8 @@ private Q_SLOTS: // ****************************************************************************************** // Slot Event Functions // ****************************************************************************************** - void edited_name(); - void edited_email(); + void editedName(); + void editedEmail(); private: /// Contains all the configuration data for the setup assistant diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index 1ed20f2b44..ec78d531e6 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -566,7 +566,7 @@ bool ConfigurationFilesWidget::loadGenFiles() bool ConfigurationFilesWidget::checkDependencies() { QStringList dependencies; - bool requiredActions = false; + bool required_actions = false; // Check that at least 1 planning group exists if (config_data_->srdf_->groups_.empty()) @@ -597,18 +597,18 @@ bool ConfigurationFilesWidget::checkDependencies() { // There is no name or it consists of whitespaces only dependencies << "No author name added"; - requiredActions = true; + required_actions = true; } // Check that email information is filled - QRegExp mailRegex("\\b[A-Z0-9._%+-]+@[A-Z0-9.-]+\\.[A-Z]{2,4}\\b"); - mailRegex.setCaseSensitivity(Qt::CaseInsensitive); - mailRegex.setPatternSyntax(QRegExp::RegExp); - QString testEmail = QString::fromStdString(config_data_->author_email_); - if (!mailRegex.exactMatch(testEmail)) + QRegExp mail_regex("\\b[A-Z0-9._%+-]+@[A-Z0-9.-]+\\.[A-Z]{2,4}\\b"); + mail_regex.setCaseSensitivity(Qt::CaseInsensitive); + mail_regex.setPatternSyntax(QRegExp::RegExp); + QString test_email = QString::fromStdString(config_data_->author_email_); + if (!mail_regex.exactMatch(test_email)) { dependencies << "No valid email address added"; - requiredActions = true; + required_actions = true; } // Display all accumumlated errors: @@ -616,7 +616,7 @@ bool ConfigurationFilesWidget::checkDependencies() { // Create a dependency message QString dep_message; - if (!requiredActions) + if (!required_actions) { dep_message = "Some setup steps have not been completed. None of the steps are required, but here is a reminder " "of what was not filled in, just in case something was forgotten:
    "; @@ -632,7 +632,7 @@ bool ConfigurationFilesWidget::checkDependencies() dep_message.append("
  • ").append(dependencies.at(i)).append("
    • "); } - if (!requiredActions) + if (!required_actions) { dep_message.append("

Press Ok to continue generating files."); if (QMessageBox::question(this, "Incomplete MoveIt! Setup Assistant Steps", dep_message, diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp index d5bf9799f1..33240ed4d6 100644 --- a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp +++ b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp @@ -563,8 +563,8 @@ bool DefaultCollisionsWidget::eventFilter(QObject* object, QEvent* event) } else if (event->type() == QEvent::KeyPress) { - QKeyEvent* keyEvent = static_cast(event); - if (keyEvent->key() != Qt::Key_Space) + QKeyEvent* key_event = static_cast(event); + if (key_event->key() != Qt::Key_Space) return false; toggleSelection(selection_model_->selection()); diff --git a/moveit_setup_assistant/src/widgets/double_list_widget.cpp b/moveit_setup_assistant/src/widgets/double_list_widget.cpp index c711150ff5..59e550ba29 100644 --- a/moveit_setup_assistant/src/widgets/double_list_widget.cpp +++ b/moveit_setup_assistant/src/widgets/double_list_widget.cpp @@ -250,8 +250,8 @@ void DoubleListWidget::selectDataButtonClicked() for (int i = 0; i < selected.size(); i++) { std::string name = selected[i]->text().toStdString(); - bool alreadyExists = false; - int rowToAdd = 0; + bool already_exists = false; + int row_to_add = 0; // Check if this selected joint is already in the selected joint table for (int r = 0; r < selected_data_table_->rowCount(); r++) @@ -260,19 +260,19 @@ void DoubleListWidget::selectDataButtonClicked() if (item->text().toStdString() == name) { - alreadyExists = true; + already_exists = true; break; } - rowToAdd = r + 1; + row_to_add = r + 1; } // This joint needs to be added to the selected joint table - if (!alreadyExists) + if (!already_exists) { selected_data_table_->setRowCount(selected_data_table_->rowCount() + 1); - QTableWidgetItem* newItem = new QTableWidgetItem(name.c_str()); - newItem->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable); - selected_data_table_->setItem(rowToAdd, 0, newItem); + QTableWidgetItem* new_item = new QTableWidgetItem(name.c_str()); + new_item->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable); + selected_data_table_->setItem(row_to_add, 0, new_item); } } diff --git a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp index c009da7fec..1617fcdc76 100644 --- a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp +++ b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp @@ -201,14 +201,14 @@ QWidget* EndEffectorsWidget::createEditWidget() controls_layout->addWidget(spacer); // Save - QPushButton* btn_save_ = new QPushButton("&Save", this); + btn_save_ = new QPushButton("&Save", this); btn_save_->setMaximumWidth(200); connect(btn_save_, SIGNAL(clicked()), this, SLOT(doneEditing())); controls_layout->addWidget(btn_save_); controls_layout->setAlignment(btn_save_, Qt::AlignRight); // Cancel - QPushButton* btn_cancel_ = new QPushButton("&Cancel", this); + btn_cancel_ = new QPushButton("&Cancel", this); btn_cancel_->setMaximumWidth(200); connect(btn_cancel_, SIGNAL(clicked()), this, SLOT(cancelEditing())); controls_layout->addWidget(btn_cancel_); @@ -547,11 +547,11 @@ void EndEffectorsWidget::doneEditing() config_data_->changes |= MoveItConfigData::END_EFFECTORS; // Save the new effector name or create the new effector ---------------------------- - bool isNew = false; + bool is_new = false; if (searched_data == nullptr) // create new { - isNew = true; + is_new = true; searched_data = new srdf::Model::EndEffector(); } @@ -563,7 +563,7 @@ void EndEffectorsWidget::doneEditing() searched_data->parent_group_ = parent_group_name_field_->currentText().toStdString(); // Insert new effectors into group state vector -------------------------- - if (isNew) + if (is_new) { config_data_->srdf_->end_effectors_.push_back(*searched_data); } diff --git a/moveit_setup_assistant/src/widgets/group_edit_widget.cpp b/moveit_setup_assistant/src/widgets/group_edit_widget.cpp index 7962aff066..5f7ed654b8 100644 --- a/moveit_setup_assistant/src/widgets/group_edit_widget.cpp +++ b/moveit_setup_assistant/src/widgets/group_edit_widget.cpp @@ -285,10 +285,10 @@ void GroupEditWidget::setSelected(const std::string& group_name) void GroupEditWidget::loadKinematicPlannersComboBox() { // Only load this combo box once - static bool hasLoaded = false; - if (hasLoaded) + static bool has_loaded = false; + if (has_loaded) return; - hasLoaded = true; + has_loaded = true; // Remove all old items kinematics_solver_field_->clear(); diff --git a/moveit_setup_assistant/src/widgets/header_widget.cpp b/moveit_setup_assistant/src/widgets/header_widget.cpp index ea6ee0b686..e42d06f5e1 100644 --- a/moveit_setup_assistant/src/widgets/header_widget.cpp +++ b/moveit_setup_assistant/src/widgets/header_widget.cpp @@ -129,7 +129,7 @@ LoadPathWidget::LoadPathWidget(const QString& title, const QString& instructions // Button QPushButton* browse_button = new QPushButton(this); browse_button->setText("Browse"); - connect(browse_button, SIGNAL(clicked()), this, SLOT(btn_file_dialog())); + connect(browse_button, SIGNAL(clicked()), this, SLOT(btnFileDialog())); hlayout->addWidget(browse_button); // Add horizontal layer to verticle layer @@ -141,7 +141,7 @@ LoadPathWidget::LoadPathWidget(const QString& title, const QString& instructions // ****************************************************************************************** // Load the file dialog // ****************************************************************************************** -void LoadPathWidget::btn_file_dialog() +void LoadPathWidget::btnFileDialog() { QString path; if (dir_only_) // only allow user to select a directory diff --git a/moveit_setup_assistant/src/widgets/header_widget.h b/moveit_setup_assistant/src/widgets/header_widget.h index d794955717..40793d36a0 100644 --- a/moveit_setup_assistant/src/widgets/header_widget.h +++ b/moveit_setup_assistant/src/widgets/header_widget.h @@ -80,7 +80,7 @@ class LoadPathWidget : public QFrame private Q_SLOTS: /// Load the file dialog - void btn_file_dialog(); + void btnFileDialog(); public: /// Constructor diff --git a/moveit_setup_assistant/src/widgets/navigation_widget.cpp b/moveit_setup_assistant/src/widgets/navigation_widget.cpp index 141b4cbcd9..d9cef1fa43 100644 --- a/moveit_setup_assistant/src/widgets/navigation_widget.cpp +++ b/moveit_setup_assistant/src/widgets/navigation_widget.cpp @@ -123,7 +123,7 @@ QSize NavDelegate::sizeHint(const QStyleOptionViewItem& option, const QModelInde void NavDelegate::paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const { - const bool isSelected = option.state & QStyle::State_Selected; + const bool is_selected = option.state & QStyle::State_Selected; const QPalette& palette = QApplication::palette(); QString nav_name = displayText(index.data(), option.locale); @@ -131,21 +131,21 @@ void NavDelegate::paint(QPainter* painter, const QStyleOptionViewItem& option, c painter->save(); // draw background gradient - QLinearGradient backgroundGradient(option.rect.topLeft(), option.rect.bottomLeft()); - if (isSelected) + QLinearGradient background_gradient(option.rect.topLeft(), option.rect.bottomLeft()); + if (is_selected) { - backgroundGradient.setColorAt(0, palette.color(QPalette::Highlight).lighter(125)); - backgroundGradient.setColorAt(1, palette.color(QPalette::Highlight)); - painter->fillRect(option.rect, QBrush(backgroundGradient)); + background_gradient.setColorAt(0, palette.color(QPalette::Highlight).lighter(125)); + background_gradient.setColorAt(1, palette.color(QPalette::Highlight)); + painter->fillRect(option.rect, QBrush(background_gradient)); } else { - backgroundGradient.setColorAt(0, palette.color(QPalette::Light)); - backgroundGradient.setColorAt(1, palette.color(QPalette::Light).darker(105)); - painter->fillRect(option.rect, QBrush(backgroundGradient)); + background_gradient.setColorAt(0, palette.color(QPalette::Light)); + background_gradient.setColorAt(1, palette.color(QPalette::Light).darker(105)); + painter->fillRect(option.rect, QBrush(background_gradient)); } - if (!isSelected) // draw shadow + if (!is_selected) // draw shadow { painter->setPen(palette.color(QPalette::Button)); painter->drawLine(option.rect.topLeft(), option.rect.topRight()); @@ -154,20 +154,20 @@ void NavDelegate::paint(QPainter* painter, const QStyleOptionViewItem& option, c painter->drawLine(option.rect.topLeft() + offset, option.rect.topRight() + offset); } - QRect textRect(option.rect.x() + 10, option.rect.y(), option.rect.width() - 10, option.rect.height()); - QFont textFont(painter->font()); - textFont.setPixelSize(14); // Set font size - painter->setFont(textFont); + QRect text_rect(option.rect.x() + 10, option.rect.y(), option.rect.width() - 10, option.rect.height()); + QFont text_font(painter->font()); + text_font.setPixelSize(14); // Set font size + painter->setFont(text_font); // Font color - if (isSelected) + if (is_selected) painter->setPen(palette.color(QPalette::HighlightedText)); else if (!option.state.testFlag(QStyle::State_Enabled)) painter->setPen(palette.color(QPalette::Dark)); else painter->setPen(palette.color(QPalette::ButtonText)); - painter->drawText(textRect, Qt::AlignLeft | Qt::AlignVCenter, nav_name); + painter->drawText(text_rect, Qt::AlignLeft | Qt::AlignVCenter, nav_name); painter->restore(); } diff --git a/moveit_setup_assistant/src/widgets/perception_widget.cpp b/moveit_setup_assistant/src/widgets/perception_widget.cpp index 45d646e6da..1b972dbc37 100644 --- a/moveit_setup_assistant/src/widgets/perception_widget.cpp +++ b/moveit_setup_assistant/src/widgets/perception_widget.cpp @@ -283,10 +283,10 @@ void PerceptionWidget::sensorPluginChanged(int index) void PerceptionWidget::loadSensorPluginsComboBox() { // Only load this combo box once - static bool hasLoaded = false; - if (hasLoaded) + static bool has_loaded = false; + if (has_loaded) return; - hasLoaded = true; + has_loaded = true; // Remove all old items sensor_plugin_field_->clear(); diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp index cc0d86e2fa..0e6fb00af1 100644 --- a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp +++ b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp @@ -73,14 +73,14 @@ namespace moveit_setup_assistant static const std::string VIS_TOPIC_NAME = "planning_components_visualization"; // Used for checking for cycles in a subgroup hierarchy -struct cycle_detector : public boost::dfs_visitor<> +struct CycleDetector : public boost::dfs_visitor<> { - cycle_detector(bool& has_cycle) : m_has_cycle(has_cycle) + CycleDetector(bool& has_cycle) : m_has_cycle(has_cycle) { } template - void back_edge(Edge /*unused*/, Graph& /*unused*/) + void backEdge(Edge /*unused*/, Graph& /*unused*/) { m_has_cycle = true; } @@ -716,20 +716,20 @@ void PlanningGroupsWidget::deleteGroup() } // delete all robot poses that use that group's name - bool haveConfirmedGroupStateDeletion = false; - bool haveDeletedGroupState = true; - while (haveDeletedGroupState) + bool have_confirmed_group_state_deletion = false; + bool have_deleted_group_state = true; + while (have_deleted_group_state) { - haveDeletedGroupState = false; + have_deleted_group_state = false; for (std::vector::iterator pose_it = config_data_->srdf_->group_states_.begin(); pose_it != config_data_->srdf_->group_states_.end(); ++pose_it) { // check if this group state depends on the currently being deleted group if (pose_it->group_ == searched_group->name_) { - if (!haveConfirmedGroupStateDeletion) + if (!have_confirmed_group_state_deletion) { - haveConfirmedGroupStateDeletion = true; + have_confirmed_group_state_deletion = true; // confirm the user wants to delete group states if (QMessageBox::question( @@ -745,27 +745,27 @@ void PlanningGroupsWidget::deleteGroup() // the user has confirmed, now delete this group state config_data_->srdf_->group_states_.erase(pose_it); - haveDeletedGroupState = true; + have_deleted_group_state = true; break; // you can only delete 1 item in vector before invalidating iterator } } } // delete all end effectors that use that group's name - bool haveConfirmedEndEffectorDeletion = false; - bool haveDeletedEndEffector = true; - while (haveDeletedEndEffector) + bool have_confirmed_end_effector_deletion = false; + bool have_deleted_end_effector = true; + while (have_deleted_end_effector) { - haveDeletedEndEffector = false; + have_deleted_end_effector = false; for (std::vector::iterator effector_it = config_data_->srdf_->end_effectors_.begin(); effector_it != config_data_->srdf_->end_effectors_.end(); ++effector_it) { // check if this group state depends on the currently being deleted group if (effector_it->component_group_ == searched_group->name_) { - if (!haveConfirmedEndEffectorDeletion) + if (!have_confirmed_end_effector_deletion) { - haveConfirmedEndEffectorDeletion = true; + have_confirmed_end_effector_deletion = true; // confirm the user wants to delete group states if (QMessageBox::question( @@ -781,7 +781,7 @@ void PlanningGroupsWidget::deleteGroup() // the user has confirmed, now delete this group state config_data_->srdf_->end_effectors_.erase(effector_it); - haveDeletedEndEffector = true; + have_deleted_end_effector = true; break; // you can only delete 1 item in vector before invalidating iterator } } @@ -1051,7 +1051,7 @@ void PlanningGroupsWidget::saveSubgroupsScreen() // Check for cycles bool has_cycle = false; - cycle_detector vis(has_cycle); + CycleDetector vis(has_cycle); boost::depth_first_search(g, visitor(vis)); if (has_cycle) diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp index da3702efd3..894954889e 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp @@ -261,14 +261,14 @@ QWidget* RobotPosesWidget::createEditWidget() controls_layout->addWidget(spacer); // Save - QPushButton* btn_save_ = new QPushButton("&Save", this); + btn_save_ = new QPushButton("&Save", this); btn_save_->setMaximumWidth(200); connect(btn_save_, SIGNAL(clicked()), this, SLOT(doneEditing())); controls_layout->addWidget(btn_save_); controls_layout->setAlignment(btn_save_, Qt::AlignRight); // Cancel - QPushButton* btn_cancel_ = new QPushButton("&Cancel", this); + btn_cancel_ = new QPushButton("&Cancel", this); btn_cancel_->setMaximumWidth(200); connect(btn_cancel_, SIGNAL(clicked()), this, SLOT(cancelEditing())); controls_layout->addWidget(btn_cancel_); @@ -659,11 +659,11 @@ void RobotPosesWidget::doneEditing() config_data_->changes |= MoveItConfigData::POSES; // Save the new pose name or create the new pose ---------------------------- - bool isNew = false; + bool is_new = false; if (searched_data == nullptr) // create new { - isNew = true; + is_new = true; searched_data = new srdf::Model::GroupState(); } @@ -693,7 +693,7 @@ void RobotPosesWidget::doneEditing() } // Insert new poses into group state vector -------------------------- - if (isNew) + if (is_new) { config_data_->srdf_->group_states_.push_back(*searched_data); delete searched_data; diff --git a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp index 791735ac7f..0c84714b39 100644 --- a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp +++ b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp @@ -356,8 +356,8 @@ void ROSControllersWidget::deleteController() return; // Get the user custom properties of the currently selected row - int type_ = item->data(0, Qt::UserRole).value(); - if (type_ == 0) + int type = item->data(0, Qt::UserRole).value(); + if (type == 0) controller_name = item->text(0).toUtf8().constData(); } diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp index 89d272ad9a..8a8fadbd27 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp +++ b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp @@ -530,9 +530,9 @@ bool StartScreenWidget::loadNewFiles() // ****************************************************************************************** bool StartScreenWidget::loadURDFFile(const std::string& urdf_file_path, const std::string& xacro_args) { - const std::vector xacro_args_ = { xacro_args }; + const std::vector vec_xacro_args = { xacro_args }; - if (!rdf_loader::RDFLoader::loadXmlFileToString(config_data_->urdf_string_, urdf_file_path, xacro_args_)) + if (!rdf_loader::RDFLoader::loadXmlFileToString(config_data_->urdf_string_, urdf_file_path, vec_xacro_args)) { QMessageBox::warning(this, "Error Loading Files", QString("URDF/COLLADA file not found: ").append(urdf_file_path.c_str())); @@ -647,7 +647,7 @@ bool StartScreenWidget::extractPackageNameFromPath() for (fs::path::iterator it = urdf_directory.begin(); it != urdf_directory.end(); ++it) path_parts.push_back(it->native()); - bool packageFound = false; + bool package_found = false; // reduce the generated directoy path's folder count by 1 each loop for (int segment_length = path_parts.size(); segment_length > 0; --segment_length) @@ -684,13 +684,13 @@ bool StartScreenWidget::extractPackageNameFromPath() // end the search segment_length = 0; - packageFound = true; + package_found = true; break; } } // Assign data to moveit_config_data - if (!packageFound) + if (!package_found) { // No package name found, we must be outside ROS config_data_->urdf_pkg_name_ = ""; diff --git a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp index 7a4b711b0d..1b488b814b 100644 --- a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp +++ b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp @@ -199,18 +199,18 @@ QWidget* VirtualJointsWidget::createEditWidget() controls_layout->addWidget(spacer); // Save - QPushButton* btn_save_ = new QPushButton("&Save", this); - btn_save_->setMaximumWidth(200); - connect(btn_save_, SIGNAL(clicked()), this, SLOT(doneEditing())); - controls_layout->addWidget(btn_save_); - controls_layout->setAlignment(btn_save_, Qt::AlignRight); + QPushButton* btn_save = new QPushButton("&Save", this); + btn_save->setMaximumWidth(200); + connect(btn_save, SIGNAL(clicked()), this, SLOT(doneEditing())); + controls_layout->addWidget(btn_save); + controls_layout->setAlignment(btn_save, Qt::AlignRight); // Cancel - QPushButton* btn_cancel_ = new QPushButton("&Cancel", this); - btn_cancel_->setMaximumWidth(200); - connect(btn_cancel_, SIGNAL(clicked()), this, SLOT(cancelEditing())); - controls_layout->addWidget(btn_cancel_); - controls_layout->setAlignment(btn_cancel_, Qt::AlignRight); + QPushButton* btn_cancel = new QPushButton("&Cancel", this); + btn_cancel->setMaximumWidth(200); + connect(btn_cancel, SIGNAL(clicked()), this, SLOT(cancelEditing())); + controls_layout->addWidget(btn_cancel); + controls_layout->setAlignment(btn_cancel, Qt::AlignRight); // Add layout layout->addLayout(controls_layout); @@ -504,11 +504,11 @@ void VirtualJointsWidget::doneEditing() config_data_->changes |= MoveItConfigData::VIRTUAL_JOINTS; // Save the new vjoint name or create the new vjoint ---------------------------- - bool isNew = false; + bool is_new = false; if (searched_data == nullptr) // create new { - isNew = true; + is_new = true; searched_data = new srdf::Model::VirtualJoint(); } @@ -521,7 +521,7 @@ void VirtualJointsWidget::doneEditing() bool emit_frame_notice = false; // Insert new vjoints into group state vector -------------------------- - if (isNew) + if (is_new) { if (searched_data->child_link_ == config_data_->getRobotModel()->getRootLinkName()) emit_frame_notice = true; diff --git a/moveit_setup_assistant/test/moveit_config_data_test.cpp b/moveit_setup_assistant/test/moveit_config_data_test.cpp index a7285b8c8d..2e5d3e00a8 100644 --- a/moveit_setup_assistant/test/moveit_config_data_test.cpp +++ b/moveit_setup_assistant/test/moveit_config_data_test.cpp @@ -82,41 +82,41 @@ TEST_F(MoveItConfigData, ReadingControllers) boost::filesystem::path res_path(MOVEIT_TEST_RESOURCES_DIR); // Contains all the configuration data for the setup assistant - moveit_setup_assistant::MoveItConfigDataPtr config_data_; + moveit_setup_assistant::MoveItConfigDataPtr config_data; - config_data_.reset(new moveit_setup_assistant::MoveItConfigData()); - config_data_->srdf_->srdf_model_ = srdf_model; - config_data_->setRobotModel(robot_model); + config_data.reset(new moveit_setup_assistant::MoveItConfigData()); + config_data->srdf_->srdf_model_ = srdf_model; + config_data->setRobotModel(robot_model); // Initially no controllers - EXPECT_EQ(config_data_->getROSControllers().size(), 0u); + EXPECT_EQ(config_data->getROSControllers().size(), 0u); // Adding default controllers, a controller for each planning group - config_data_->addDefaultControllers(); + config_data->addDefaultControllers(); // Number of the planning groups defined in the model srdf - size_t group_count = config_data_->srdf_->srdf_model_->getGroups().size(); + size_t group_count = config_data->srdf_->srdf_model_->getGroups().size(); // Test that addDefaultControllers() did accually add a controller for the new_group - EXPECT_EQ(config_data_->getROSControllers().size(), group_count); + EXPECT_EQ(config_data->getROSControllers().size(), group_count); // Temporary file used during the test and is deleted when the test is finished char test_file[] = "/tmp/msa_unittest_ros_controller.yaml"; // ros_controller.yaml written correctly - EXPECT_EQ(config_data_->outputROSControllersYAML(test_file), true); + EXPECT_EQ(config_data->outputROSControllersYAML(test_file), true); // Reset MoveIt! config MoveItConfigData - config_data_.reset(new moveit_setup_assistant::MoveItConfigData()); + config_data.reset(new moveit_setup_assistant::MoveItConfigData()); // Initially no controllers - EXPECT_EQ(config_data_->getROSControllers().size(), 0u); + EXPECT_EQ(config_data->getROSControllers().size(), 0u); // ros_controllers.yaml read correctly - EXPECT_EQ(config_data_->inputROSControllersYAML(test_file), true); + EXPECT_EQ(config_data->inputROSControllersYAML(test_file), true); // ros_controllers.yaml parsed correctly - EXPECT_EQ(config_data_->getROSControllers().size(), group_count); + EXPECT_EQ(config_data->getROSControllers().size(), group_count); // Remove ros_controllers.yaml temp file which was used in testing boost::filesystem::remove(test_file); @@ -126,26 +126,26 @@ TEST_F(MoveItConfigData, ReadingControllers) TEST_F(MoveItConfigData, ReadingSensorsConfig) { // Contains all the config data for the setup assistant - moveit_setup_assistant::MoveItConfigDataPtr config_data_; - config_data_.reset(new moveit_setup_assistant::MoveItConfigData()); + moveit_setup_assistant::MoveItConfigDataPtr config_data; + config_data.reset(new moveit_setup_assistant::MoveItConfigData()); - boost::filesystem::path setup_assistant_path(config_data_->setup_assistant_path_); + boost::filesystem::path setup_assistant_path(config_data->setup_assistant_path_); // Before parsing, no config available - EXPECT_EQ(config_data_->getSensorPluginConfig().size(), 0u); + EXPECT_EQ(config_data->getSensorPluginConfig().size(), 0u); // Read the file containing the default config parameters - config_data_->input3DSensorsYAML( + config_data->input3DSensorsYAML( (setup_assistant_path / "templates/moveit_config_pkg_template/config/sensors_3d.yaml").string()); // Default config for the two available sensor plugins // Make sure both are parsed correctly - EXPECT_EQ(config_data_->getSensorPluginConfig().size(), 2u); + EXPECT_EQ(config_data->getSensorPluginConfig().size(), 2u); - EXPECT_EQ(config_data_->getSensorPluginConfig()[0]["sensor_plugin"].getValue(), + EXPECT_EQ(config_data->getSensorPluginConfig()[0]["sensor_plugin"].getValue(), std::string("occupancy_map_monitor/PointCloudOctomapUpdater")); - EXPECT_EQ(config_data_->getSensorPluginConfig()[1]["sensor_plugin"].getValue(), + EXPECT_EQ(config_data->getSensorPluginConfig()[1]["sensor_plugin"].getValue(), std::string("occupancy_map_monitor/DepthImageOctomapUpdater")); } From 4e7f360a04d30c11b242544d25c4b05385fc64f0 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Fri, 15 Mar 2019 11:01:35 -0600 Subject: [PATCH 7/8] New isValidVelocityMove() for checking time between two waypoints given velocity (#684) * New isValidVelocityMove() for checking time between two waypoints given velocity * Move isValidVelocityMove() to JointModelGroup * Update moveit_core/robot_model/src/joint_model_group.cpp --- .../moveit/robot_model/joint_model_group.h | 8 +++ .../robot_model/src/joint_model_group.cpp | 72 +++++++++++++++---- moveit_core/robot_state/src/robot_state.cpp | 1 - 3 files changed, 66 insertions(+), 15 deletions(-) diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h index b4fbaeda76..d46e1fc748 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h @@ -567,6 +567,14 @@ class JointModelGroup /** \brief Print information about the constructed model */ void printGroupInfo(std::ostream& out = std::cout) const; + /** \brief Check that the time to move between two waypoints is sufficient given velocity limits */ + bool isValidVelocityMove(const std::vector& from_joint_pose, const std::vector& to_joint_pose, + double dt) const; + + /** \brief Check that the time to move between two waypoints is sufficient given velocity limits */ + bool isValidVelocityMove(const double* from_joint_pose, const double* to_joint_pose, std::size_t array_size, + double dt) const; + protected: bool computeIKIndexBijection(const std::vector& ik_jnames, std::vector& joint_bijection) const; diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index 24e743c562..d5583ea35a 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -96,6 +96,8 @@ bool jointPrecedes(const JointModel* a, const JointModel* b) } } // namespace +const std::string LOGNAME = "robot_model.jmg"; + JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Model::Group& config, const std::vector& unsorted_group_joints, const RobotModel* parent_model) @@ -292,7 +294,7 @@ const LinkModel* JointModelGroup::getLinkModel(const std::string& name) const LinkModelMapConst::const_iterator it = link_model_map_.find(name); if (it == link_model_map_.end()) { - ROS_ERROR_NAMED("robot_model.jmg", "Link '%s' not found in group '%s'", name.c_str(), name_.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Link '%s' not found in group '%s'", name.c_str(), name_.c_str()); return nullptr; } return it->second; @@ -303,7 +305,7 @@ const JointModel* JointModelGroup::getJointModel(const std::string& name) const JointModelMapConst::const_iterator it = joint_model_map_.find(name); if (it == joint_model_map_.end()) { - ROS_ERROR_NAMED("robot_model.jmg", "Joint '%s' not found in group '%s'", name.c_str(), name_.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Joint '%s' not found in group '%s'", name.c_str(), name_.c_str()); return nullptr; } return it->second; @@ -345,8 +347,7 @@ void JointModelGroup::getVariableRandomPositionsNearBy( if (iter != distance_map.end()) distance = iter->second; else - ROS_WARN_NAMED("robot_model.jmg", "Did not pass in distance for '%s'", - active_joint_model_vector_[i]->getName().c_str()); + ROS_WARN_NAMED(LOGNAME, "Did not pass in distance for '%s'", active_joint_model_vector_[i]->getName().c_str()); active_joint_model_vector_[i]->getVariableRandomPositionsNearBy( rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i], near + active_joint_model_start_index_[i], distance); @@ -502,7 +503,7 @@ bool JointModelGroup::getEndEffectorTips(std::vector& tips) co const JointModelGroup* eef = parent_model_->getEndEffector(name); if (!eef) { - ROS_ERROR_NAMED("robot_model.jmg", "Unable to find joint model group for eef"); + ROS_ERROR_NAMED(LOGNAME, "Unable to find joint model group for eef"); return false; } const std::string& eef_parent = eef->getEndEffectorParentGroup().second; @@ -510,7 +511,7 @@ bool JointModelGroup::getEndEffectorTips(std::vector& tips) co const LinkModel* eef_link = parent_model_->getLinkModel(eef_parent); if (!eef_link) { - ROS_ERROR_NAMED("robot_model.jmg", "Unable to find end effector link for eef"); + ROS_ERROR_NAMED(LOGNAME, "Unable to find end effector link for eef"); return false; } // insert eef_link into tips, maintaining a *sorted* vector, thus enabling use of std::lower_bound @@ -528,10 +529,10 @@ const LinkModel* JointModelGroup::getOnlyOneEndEffectorTip() const if (tips.size() == 1) return tips.front(); else if (tips.size() > 1) - ROS_ERROR_NAMED("robot_model.jmg", "More than one end effector tip found for joint model group, " - "so cannot return only one"); + ROS_ERROR_NAMED(LOGNAME, "More than one end effector tip found for joint model group, " + "so cannot return only one"); else - ROS_ERROR_NAMED("robot_model.jmg", "No end effector tips found in joint model group"); + ROS_ERROR_NAMED(LOGNAME, "No end effector tips found in joint model group"); return nullptr; } @@ -540,7 +541,7 @@ int JointModelGroup::getVariableGroupIndex(const std::string& variable) const VariableIndexMap::const_iterator it = joint_variables_index_map_.find(variable); if (it == joint_variables_index_map_.end()) { - ROS_ERROR_NAMED("robot_model.jmg", "Variable '%s' is not part of group '%s'", variable.c_str(), name_.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Variable '%s' is not part of group '%s'", variable.c_str(), name_.c_str()); return -1; } return it->second; @@ -567,8 +568,8 @@ bool JointModelGroup::computeIKIndexBijection(const std::vector& ik // skip reported fixed joints if (hasJointModel(ik_jnames[i]) && getJointModel(ik_jnames[i])->getType() == JointModel::FIXED) continue; - ROS_ERROR_NAMED("robot_model.jmg", "IK solver computes joint values for joint '%s' " - "but group '%s' does not contain such a joint.", + ROS_ERROR_NAMED(LOGNAME, "IK solver computes joint values for joint '%s' " + "but group '%s' does not contain such a joint.", ik_jnames[i].c_str(), getName().c_str()); return false; } @@ -620,7 +621,7 @@ bool JointModelGroup::canSetStateFromIK(const std::string& tip) const if (tip_frames.empty()) { - ROS_DEBUG_NAMED("robot_model.jmg", "Group %s has no tip frame(s)", name_.c_str()); + ROS_DEBUG_NAMED(LOGNAME, "Group %s has no tip frame(s)", name_.c_str()); return false; } @@ -630,7 +631,7 @@ bool JointModelGroup::canSetStateFromIK(const std::string& tip) const // remove frame reference, if specified const std::string& tip_local = tip[0] == '/' ? tip.substr(1) : tip; const std::string& tip_frame_local = tip_frames[i][0] == '/' ? tip_frames[i].substr(1) : tip_frames[i]; - ROS_DEBUG_NAMED("robot_model.jmg", "comparing input tip: %s to this groups tip: %s ", tip_local.c_str(), + ROS_DEBUG_NAMED(LOGNAME, "comparing input tip: %s to this groups tip: %s ", tip_local.c_str(), tip_frame_local.c_str()); // Check if the IK solver's tip is the same as the frame of inquiry @@ -717,5 +718,48 @@ void JointModelGroup::printGroupInfo(std::ostream& out) const out << std::endl; } +bool JointModelGroup::isValidVelocityMove(const std::vector& from_joint_pose, + const std::vector& to_joint_pose, double dt) const +{ + // Check for equal sized arrays + if (from_joint_pose.size() != to_joint_pose.size()) + { + ROS_ERROR_NAMED(LOGNAME, "To and from joint poses are of different sizes."); + return false; + } + + return isValidVelocityMove(&from_joint_pose[0], &to_joint_pose[0], from_joint_pose.size(), dt); +} + +bool JointModelGroup::isValidVelocityMove(const double* from_joint_pose, const double* to_joint_pose, + std::size_t array_size, double dt) const +{ + const std::vector& bounds = getActiveJointModelsBounds(); + const std::vector& bij = getKinematicsSolverJointBijection(); + + for (std::size_t i = 0; i < array_size; ++i) + { + double dtheta = std::abs(from_joint_pose[i] - to_joint_pose[i]); + const std::vector* var_bounds = bounds[bij[i]]; + + if (var_bounds->size() != 1) + { + // TODO(davetcoleman) Support multiple variables + ROS_ERROR_NAMED(LOGNAME, "Attempting to check velocity bounds for waypoint move with joints that have multiple " + "variables"); + return false; + } + const double max_velocity = (*var_bounds)[0].max_velocity_; + + double max_dtheta = dt * max_velocity; + if (dtheta > max_dtheta) + { + ROS_DEBUG_STREAM_NAMED(LOGNAME, "Not valid velocity move because of joint " << i); + return false; + } + } + + return true; +} } // end of namespace core } // end of namespace moveit diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index acbdf3a5ce..0f4f8d7957 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -830,7 +830,6 @@ bool RobotState::isValidVelocityMove(const RobotState& other, const JointModelGr return false; } } - return true; } From d649771a65b10f672261197da527a7bae259ed05 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 16 Mar 2019 10:15:05 +0000 Subject: [PATCH 8/8] moveit_commander.MoveGroupInterface.plan() to better align with C++ MoveGroup::plan() (#790) * MoveGroupCommander.plan() now returns [success, trajectory, planning_time, error_code] --- MIGRATION.md | 1 + .../src/moveit_commander/move_group.py | 66 +++++++++++-------- .../test/python_moveit_commander.py | 12 ++-- .../test/python_moveit_commander_ns.py | 13 ++-- .../src/wrap_python_move_group.cpp | 9 +-- .../test/python_move_group.py | 20 ++++-- .../test/python_move_group_ns.py | 22 +++++-- .../test/robot_state_update.py | 18 +++-- 8 files changed, 104 insertions(+), 57 deletions(-) diff --git a/MIGRATION.md b/MIGRATION.md index 25f6d842de..dd07c8b17c 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -3,6 +3,7 @@ API changes in MoveIt! releases ## ROS Noetic (upcoming changes in master) +- Extended the return value of `MoveitCommander.MoveGroup.plan()` from `trajectory` to a tuple of `(success, trajectory, planning_time, error_code)` to better match the C++ MoveGroupInterface ([790](https://github.com/ros-planning/moveit/pull/790/)) ## ROS Melodic diff --git a/moveit_commander/src/moveit_commander/move_group.py b/moveit_commander/src/moveit_commander/move_group.py index da598cf33c..c305c16eae 100644 --- a/moveit_commander/src/moveit_commander/move_group.py +++ b/moveit_commander/src/moveit_commander/move_group.py @@ -42,6 +42,7 @@ from .exception import MoveItCommanderException import moveit_commander.conversions as conversions + class MoveGroupCommander(object): """ Execution of simple commands for a particular group @@ -106,14 +107,14 @@ def get_current_joint_values(self): """ Get the current configuration of the group as a list (these are values published on /joint_states) """ return self._g.get_current_joint_values() - def get_current_pose(self, end_effector_link = ""): + def get_current_pose(self, end_effector_link=""): """ Get the current pose of the end-effector of the group. Throws an exception if there is not end-effector. """ if len(end_effector_link) > 0 or self.has_end_effector_link(): return conversions.list_to_pose_stamped(self._g.get_current_pose(end_effector_link), self.get_planning_frame()) else: raise MoveItCommanderException("There is no end effector to get the pose of") - def get_current_rpy(self, end_effector_link = ""): + def get_current_rpy(self, end_effector_link=""): """ Get a list of 3 elements defining the [roll, pitch, yaw] of the end-effector. Throws an exception if there is not end-effector. """ if len(end_effector_link) > 0 or self.has_end_effector_link(): return self._g.get_current_rpy(end_effector_link) @@ -123,7 +124,7 @@ def get_current_rpy(self, end_effector_link = ""): def get_random_joint_values(self): return self._g.get_random_joint_values() - def get_random_pose(self, end_effector_link = ""): + def get_random_pose(self, end_effector_link=""): if len(end_effector_link) > 0 or self.has_end_effector_link(): return conversions.list_to_pose_stamped(self._g.get_random_pose(end_effector_link), self.get_planning_frame()) else: @@ -158,7 +159,7 @@ def set_start_state(self, msg): def get_joint_value_target(self): return self._g.get_joint_value_target() - def set_joint_value_target(self, arg1, arg2 = None, arg3 = None): + def set_joint_value_target(self, arg1, arg2=None, arg3=None): """ Specify a target joint configuration for the group. - if the type of arg1 is one of the following: dict, list, JointState message, then no other arguments should be provided. @@ -230,7 +231,7 @@ def set_joint_value_target(self, arg1, arg2 = None, arg3 = None): raise MoveItCommanderException("Unsupported argument of type %s" % type(arg1)) - def set_rpy_target(self, rpy, end_effector_link = ""): + def set_rpy_target(self, rpy, end_effector_link=""): """ Specify a target orientation for the end-effector. Any position of the end-effector is acceptable.""" if len(end_effector_link) > 0 or self.has_end_effector_link(): if len(rpy) == 3: @@ -241,7 +242,7 @@ def set_rpy_target(self, rpy, end_effector_link = ""): else: raise MoveItCommanderException("There is no end effector to set the pose for") - def set_orientation_target(self, q, end_effector_link = ""): + def set_orientation_target(self, q, end_effector_link=""): """ Specify a target orientation for the end-effector. Any position of the end-effector is acceptable.""" if len(end_effector_link) > 0 or self.has_end_effector_link(): if len(q) == 4: @@ -252,7 +253,7 @@ def set_orientation_target(self, q, end_effector_link = ""): else: raise MoveItCommanderException("There is no end effector to set the pose for") - def set_position_target(self, xyz, end_effector_link = ""): + def set_position_target(self, xyz, end_effector_link=""): """ Specify a target position for the end-effector. Any orientation of the end-effector is acceptable.""" if len(end_effector_link) > 0 or self.has_end_effector_link(): if not self._g.set_position_target(xyz[0], xyz[1], xyz[2], end_effector_link): @@ -260,7 +261,7 @@ def set_position_target(self, xyz, end_effector_link = ""): else: raise MoveItCommanderException("There is no end effector to set the pose for") - def set_pose_target(self, pose, end_effector_link = ""): + def set_pose_target(self, pose, end_effector_link=""): """ Set the pose of the end-effector, if one is available. The expected input is a Pose message, a PoseStamped message or a list of 6 floats:""" """ [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """ if len(end_effector_link) > 0 or self.has_end_effector_link(): @@ -279,7 +280,7 @@ def set_pose_target(self, pose, end_effector_link = ""): else: raise MoveItCommanderException("There is no end effector to set the pose for") - def set_pose_targets(self, poses, end_effector_link = ""): + def set_pose_targets(self, poses, end_effector_link=""): """ Set the pose of the end-effector, if one is available. The expected input is a list of poses. Each pose can be a Pose message, a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """ if len(end_effector_link) > 0 or self.has_end_effector_link(): if not self._g.set_pose_targets([conversions.pose_to_list(p) if type(p) is Pose else p for p in poses], end_effector_link): @@ -287,7 +288,7 @@ def set_pose_targets(self, poses, end_effector_link = ""): else: raise MoveItCommanderException("There is no end effector to set poses for") - def shift_pose_target(self, axis, value, end_effector_link = ""): + def shift_pose_target(self, axis, value, end_effector_link=""): """ Get the current pose of the end effector, add value to the corresponding axis (0..5: X, Y, Z, R, P, Y) and set the new pose as the pose target """ if len(end_effector_link) > 0 or self.has_end_effector_link(): pose = self._g.get_current_pose(end_effector_link) @@ -329,7 +330,7 @@ def get_named_target_values(self, target): """Get a dictionary of joint values of a named target""" return self._g.get_named_target_values(target) - def remember_joint_values(self, name, values = None): + def remember_joint_values(self, name, values=None): """ Record the specified joint configuration of the group under the specified name. If no values are specified, the current state of the group is recorded. """ if values is None: values = self.get_current_joint_values() @@ -391,7 +392,7 @@ def get_path_constraints(self): """ Get the acutal path constraints in form of a moveit_msgs.msgs.Constraints """ c = Constraints() c_str = self._g.get_path_constraints() - conversions.msg_from_string(c,c_str) + conversions.msg_from_string(c, c_str) return c def set_path_constraints(self, value): @@ -471,16 +472,16 @@ def set_max_velocity_scaling_factor(self, value): if value > 0 and value <= 1: self._g.set_max_velocity_scaling_factor(value) else: - raise MoveItCommanderException("Expected value in the range from 0 to 1 for scaling factor" ) + raise MoveItCommanderException("Expected value in the range from 0 to 1 for scaling factor") def set_max_acceleration_scaling_factor(self, value): """ Set a scaling factor for optionally reducing the maximum joint acceleration. Allowed values are in (0,1]. """ if value > 0 and value <= 1: self._g.set_max_acceleration_scaling_factor(value) else: - raise MoveItCommanderException("Expected value in the range from 0 to 1 for scaling factor" ) + raise MoveItCommanderException("Expected value in the range from 0 to 1 for scaling factor") - def go(self, joints = None, wait = True): + def go(self, joints=None, wait=True): """ Set the target of the group and then move the group to the specified target """ if type(joints) is bool: wait = joints @@ -492,34 +493,43 @@ def go(self, joints = None, wait = True): elif type(joints) is Pose: self.set_pose_target(joints) - elif not joints is None: + elif joints is not None: try: self.set_joint_value_target(self.get_remembered_joint_values()[joints]) - except: + except MoveItCommanderException: self.set_joint_value_target(joints) if wait: return self._g.move() else: return self._g.async_move() - def plan(self, joints = None): - """ Return a motion plan (a RobotTrajectory) to the set goal state (or specified by the joints argument) """ + def plan(self, joints=None): + """ Return a tuple of the motion planning results such as + (success flag : boolean, trajectory message : RobotTrajectory, + planning time : float, error code : MoveitErrorCodes) """ if type(joints) is JointState: self.set_joint_value_target(joints) elif type(joints) is Pose: self.set_pose_target(joints) - elif not joints is None: + elif joints is not None: try: self.set_joint_value_target(self.get_remembered_joint_values()[joints]) - except: + except MoveItCommanderException: self.set_joint_value_target(joints) + + (error_code_msg, trajectory_msg, planning_time) = self._g.plan() + + error_code = MoveItErrorCodes() + error_code.deserialize(error_code_msg) plan = RobotTrajectory() - plan.deserialize(self._g.compute_plan()) - return plan + return (error_code.val == MoveItErrorCodes.SUCCESS, + plan.deserialize(trajectory_msg), + planning_time, + error_code) - def compute_cartesian_path(self, waypoints, eef_step, jump_threshold, avoid_collisions = True, path_constraints = None): + def compute_cartesian_path(self, waypoints, eef_step, jump_threshold, avoid_collisions=True, path_constraints=None): """ Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath; Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """ if path_constraints: if type(path_constraints) is Constraints: @@ -534,22 +544,22 @@ def compute_cartesian_path(self, waypoints, eef_step, jump_threshold, avoid_coll path.deserialize(ser_path) return (path, fraction) - def execute(self, plan_msg, wait = True): + def execute(self, plan_msg, wait=True): """Execute a previously planned path""" if wait: return self._g.execute(conversions.msg_to_string(plan_msg)) else: return self._g.async_execute(conversions.msg_to_string(plan_msg)) - def attach_object(self, object_name, link_name = "", touch_links = []): + def attach_object(self, object_name, link_name="", touch_links=[]): """ Given the name of an object existing in the planning scene, attach it to a link. The link used is specified by the second argument. If left unspecified, the end-effector link is used, if one is known. If there is no end-effector link, the first link in the group is used. If no link is identified, failure is reported. True is returned if an attach request was succesfully sent to the move_group node. This does not verify that the attach request also was successfuly applied by move_group.""" return self._g.attach_object(object_name, link_name, touch_links) - def detach_object(self, name = ""): + def detach_object(self, name=""): """ Given the name of a link, detach the object(s) from that link. If no such link exists, the name is interpreted as an object name. If there is no name specified, an attempt is made to detach all objects attached to any link in the group.""" return self._g.detach_object(name) - def pick(self, object_name, grasp = [], plan_only=False): + def pick(self, object_name, grasp=[], plan_only=False): """Pick the named object. A grasp message, or a list of Grasp messages can also be specified as argument.""" if type(grasp) is Grasp: return self._g.pick(object_name, conversions.msg_to_string(grasp), plan_only) diff --git a/moveit_commander/test/python_moveit_commander.py b/moveit_commander/test/python_moveit_commander.py index 6d5d23fbce..48d01ce524 100755 --- a/moveit_commander/test/python_moveit_commander.py +++ b/moveit_commander/test/python_moveit_commander.py @@ -69,7 +69,7 @@ def test_target_setting(self): self.check_target_setting((0.2,) * n) self.check_target_setting(np.zeros(n)) self.check_target_setting([0.3] * n, {name: 0.3 for name in self.group.get_active_joints()}) - self.check_target_setting([0.5] + [0.3]*(n-1), "joint_1", 0.5) + self.check_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5) def plan(self, target): self.group.set_joint_value_target(target) @@ -78,8 +78,10 @@ def plan(self, target): def test_validation(self): current = np.asarray(self.group.get_current_joint_values()) - plan1 = self.plan(current + 0.2) - plan2 = self.plan(current + 0.2) + success1, plan1, time1, err1 = self.plan(current + 0.2) + success2, plan2, time2, err2 = self.plan(current + 0.2) + self.assertTrue(success1) + self.assertTrue(success2) # first plan should execute self.assertTrue(self.group.execute(plan1)) @@ -88,12 +90,14 @@ def test_validation(self): self.assertFalse(self.group.execute(plan2)) # newly planned trajectory should execute again - plan3 = self.plan(current) + success3, plan3, time3, err3 = self.plan(current) + self.assertTrue(success3) self.assertTrue(self.group.execute(plan3)) def test_planning_scene_interface(self): planning_scene = PlanningSceneInterface() + if __name__ == '__main__': PKGNAME = 'moveit_ros_planning_interface' NODENAME = 'moveit_test_python_moveit_commander' diff --git a/moveit_commander/test/python_moveit_commander_ns.py b/moveit_commander/test/python_moveit_commander_ns.py index efaa6f03d3..43f2cdb0b1 100755 --- a/moveit_commander/test/python_moveit_commander_ns.py +++ b/moveit_commander/test/python_moveit_commander_ns.py @@ -52,7 +52,7 @@ class PythonMoveitCommanderNsTest(unittest.TestCase): @classmethod def setUpClass(self): - self.commander = RobotCommander("%srobot_description"%self.PLANNING_NS, self.PLANNING_NS) + self.commander = RobotCommander("%srobot_description" % self.PLANNING_NS, self.PLANNING_NS) self.group = self.commander.get_group(self.PLANNING_GROUP) @classmethod @@ -73,7 +73,7 @@ def test_target_setting(self): self.check_target_setting((0.2,) * n) self.check_target_setting(np.zeros(n)) self.check_target_setting([0.3] * n, {name: 0.3 for name in self.group.get_active_joints()}) - self.check_target_setting([0.5] + [0.3]*(n-1), "joint_1", 0.5) + self.check_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5) def plan(self, target): self.group.set_joint_value_target(target) @@ -82,8 +82,10 @@ def plan(self, target): def test_validation(self): current = np.asarray(self.group.get_current_joint_values()) - plan1 = self.plan(current + 0.2) - plan2 = self.plan(current + 0.2) + success1, plan1, time1, err1 = self.plan(current + 0.2) + success2, plan2, time2, err2 = self.plan(current + 0.2) + self.assertTrue(success1) + self.assertTrue(success2) # first plan should execute self.assertTrue(self.group.execute(plan1)) @@ -92,7 +94,8 @@ def test_validation(self): self.assertFalse(self.group.execute(plan2)) # newly planned trajectory should execute again - plan3 = self.plan(current) + success3, plan3, time3, err3 = self.plan(current) + self.assertTrue(success3) self.assertTrue(self.group.execute(plan3)) diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp index 3b63a48967..b926121c6f 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp @@ -425,11 +425,12 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return asyncExecute(plan) == MoveItErrorCode::SUCCESS; } - std::string getPlanPython() + bp::tuple planPython() { MoveGroupInterface::Plan plan; - MoveGroupInterface::plan(plan); - return py_bindings_tools::serializeMsg(plan.trajectory_); + moveit_msgs::MoveItErrorCodes res = MoveGroupInterface::plan(plan); + return bp::make_tuple(py_bindings_tools::serializeMsg(res), py_bindings_tools::serializeMsg(plan.trajectory_), + plan.planning_time_); } bp::tuple computeCartesianPathPython(const bp::list& waypoints, double eef_step, double jump_threshold, @@ -646,7 +647,7 @@ static void wrap_move_group_interface() &MoveGroupInterfaceWrapper::setMaxAccelerationScalingFactor); move_group_interface_class.def("set_planner_id", &MoveGroupInterfaceWrapper::setPlannerId); move_group_interface_class.def("set_num_planning_attempts", &MoveGroupInterfaceWrapper::setNumPlanningAttempts); - move_group_interface_class.def("compute_plan", &MoveGroupInterfaceWrapper::getPlanPython); + move_group_interface_class.def("plan", &MoveGroupInterfaceWrapper::planPython); move_group_interface_class.def("compute_cartesian_path", &MoveGroupInterfaceWrapper::computeCartesianPathPython); move_group_interface_class.def("compute_cartesian_path", &MoveGroupInterfaceWrapper::computeCartesianPathConstrainedPython); diff --git a/moveit_ros/planning_interface/test/python_move_group.py b/moveit_ros/planning_interface/test/python_move_group.py index 7fc2a267e1..b339fa6479 100755 --- a/moveit_ros/planning_interface/test/python_move_group.py +++ b/moveit_ros/planning_interface/test/python_move_group.py @@ -7,6 +7,7 @@ import os from moveit_ros_planning_interface._moveit_move_group_interface import MoveGroupInterface +from moveit_msgs.msg import MoveItErrorCodes class PythonMoveGroupTest(unittest.TestCase): @@ -38,13 +39,21 @@ def test_target_setting(self): def plan(self, target): self.group.set_joint_value_target(target) - return self.group.compute_plan() + return self.group.plan() def test_validation(self): current = np.asarray(self.group.get_current_joint_values()) - plan1 = self.plan(current + 0.2) - plan2 = self.plan(current + 0.2) + error_code1, plan1, time = self.plan(current + 0.2) + error_code2, plan2, time = self.plan(current + 0.2) + + # both plans should have succeeded: + error_code = MoveItErrorCodes() + error_code.deserialize(error_code1) + self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) + error_code = MoveItErrorCodes() + error_code.deserialize(error_code2) + self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) # first plan should execute self.assertTrue(self.group.execute(plan1)) @@ -53,7 +62,10 @@ def test_validation(self): self.assertFalse(self.group.execute(plan2)) # newly planned trajectory should execute again - plan3 = self.plan(current) + error_code3, plan3, time = self.plan(current) + error_code = MoveItErrorCodes() + error_code.deserialize(error_code3) + self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) self.assertTrue(self.group.execute(plan3)) diff --git a/moveit_ros/planning_interface/test/python_move_group_ns.py b/moveit_ros/planning_interface/test/python_move_group_ns.py index 13a5c21fed..b122a64600 100755 --- a/moveit_ros/planning_interface/test/python_move_group_ns.py +++ b/moveit_ros/planning_interface/test/python_move_group_ns.py @@ -44,6 +44,7 @@ import os from moveit_ros_planning_interface._moveit_move_group_interface import MoveGroupInterface +from moveit_msgs.msg import MoveItErrorCodes class PythonMoveGroupNsTest(unittest.TestCase): @@ -52,7 +53,7 @@ class PythonMoveGroupNsTest(unittest.TestCase): @classmethod def setUpClass(self): - self.group = MoveGroupInterface(self.PLANNING_GROUP, "%srobot_description"%self.PLANNING_NS, self.PLANNING_NS) + self.group = MoveGroupInterface(self.PLANNING_GROUP, "%srobot_description" % self.PLANNING_NS, self.PLANNING_NS) @classmethod def tearDown(self): @@ -76,13 +77,21 @@ def test_target_setting(self): def plan(self, target): self.group.set_joint_value_target(target) - return self.group.compute_plan() + return self.group.plan() def test_validation(self): current = np.asarray(self.group.get_current_joint_values()) - plan1 = self.plan(current + 0.2) - plan2 = self.plan(current + 0.2) + error_code1, plan1, time = self.plan(current + 0.2) + error_code2, plan2, time = self.plan(current + 0.2) + + # both plans should have succeeded: + error_code = MoveItErrorCodes() + error_code.deserialize(error_code1) + self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) + error_code = MoveItErrorCodes() + error_code.deserialize(error_code2) + self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) # first plan should execute self.assertTrue(self.group.execute(plan1)) @@ -91,8 +100,11 @@ def test_validation(self): self.assertFalse(self.group.execute(plan2)) # newly planned trajectory should execute again - plan3 = self.plan(current) + error_code3, plan3, time = self.plan(current) self.assertTrue(self.group.execute(plan3)) + error_code = MoveItErrorCodes() + error_code.deserialize(error_code3) + self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) if __name__ == '__main__': diff --git a/moveit_ros/planning_interface/test/robot_state_update.py b/moveit_ros/planning_interface/test/robot_state_update.py index f85c5328e1..a522e2a46d 100755 --- a/moveit_ros/planning_interface/test/robot_state_update.py +++ b/moveit_ros/planning_interface/test/robot_state_update.py @@ -7,6 +7,7 @@ import os from moveit_ros_planning_interface._moveit_move_group_interface import MoveGroupInterface +from moveit_msgs.msg import MoveItErrorCodes class RobotStateUpdateTest(unittest.TestCase): @@ -22,20 +23,23 @@ def tearDown(self): def plan(self, target): self.group.set_joint_value_target(target) - return self.group.compute_plan() + return self.group.plan() def test(self): current = np.asarray(self.group.get_current_joint_values()) for i in range(30): - target = current + np.random.uniform(-0.5, 0.5, size = current.shape) + target = current + np.random.uniform(-0.5, 0.5, size=current.shape) # if plan was successfully executed, current state should be reported at target - if self.group.execute(self.plan(target)): - actual = np.asarray(self.group.get_current_joint_values()) - self.assertTrue(np.allclose(target, actual, atol=1e-4, rtol=0.0)) + error_code1, plan, time = self.plan(target) + error_code = MoveItErrorCodes() + error_code.deserialize(error_code1) + if (error_code.val == MoveItErrorCodes.SUCCESS) and self.group.execute(plan): + actual = np.asarray(self.group.get_current_joint_values()) + self.assertTrue(np.allclose(target, actual, atol=1e-4, rtol=0.0)) # otherwise current state should be still the same else: - actual = np.asarray(self.group.get_current_joint_values()) - self.assertTrue(np.allclose(current, actual, atol=1e-4, rtol=0.0)) + actual = np.asarray(self.group.get_current_joint_values()) + self.assertTrue(np.allclose(current, actual, atol=1e-4, rtol=0.0)) if __name__ == '__main__':