From 8f1caae4a8bffdc3cad8b33f5f32f6671a8c0f75 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 15 May 2023 10:03:54 +0200 Subject: [PATCH] Add new clang-tidy rules and apply changes --- .clang-tidy | 8 + .../collision_detection/test/test_world.cpp | 10 +- .../src/collision_common.cpp | 12 +- .../test/pr2_arm_kinematics_plugin.cpp | 4 +- .../test/pr2_arm_kinematics_plugin.h | 2 +- .../test/test_distance_field.cpp | 18 +-- .../src/kinematic_constraint.cpp | 6 +- .../test/test_orientation_constraints.cpp | 20 +-- .../src/planning_interface.cpp | 4 +- .../src/planning_request_adapter.cpp | 2 +- .../test/test_planning_scene.cpp | 46 +++--- .../robot_model/src/joint_model_group.cpp | 2 +- moveit_core/robot_model/src/robot_model.cpp | 4 +- moveit_core/robot_state/src/conversions.cpp | 35 ++--- .../test/test_cartesian_interpolator.cpp | 146 +++++++++--------- .../test/test_kinematic_complex.cpp | 2 +- .../robot_trajectory/robot_trajectory.h | 4 +- .../robot_trajectory/src/robot_trajectory.cpp | 6 +- .../test/test_robot_trajectory.cpp | 6 +- .../time_optimal_trajectory_generation.cpp | 3 +- ...est_time_optimal_trajectory_generation.cpp | 10 +- .../include/moveit/utils/moveit_error_code.h | 2 +- .../moveit/utils/robot_model_test_utils.h | 2 +- .../utils/src/robot_model_test_utils.cpp | 2 +- .../scripts/generate_state_database.cpp | 10 +- .../joint_limits_rosparam.hpp | 46 +++--- .../joint_limits_interface_extension.h | 4 +- .../src/command_list_manager.cpp | 7 +- .../src/joint_limits_aggregator.cpp | 3 +- .../src/joint_limits_container.cpp | 5 +- .../src/limits_container.cpp | 5 +- .../src/move_group_sequence_action.cpp | 6 +- .../src/move_group_sequence_service.cpp | 6 +- .../src/pilz_industrial_motion_planner.cpp | 7 +- .../src/planning_context_loader_circ.cpp | 3 +- .../src/planning_context_loader_lin.cpp | 3 +- .../src/planning_context_loader_ptp.cpp | 3 +- .../trajectory_blender_transition_window.cpp | 2 +- .../src/trajectory_functions.cpp | 2 +- .../src/trajectory_generator.cpp | 5 +- .../src/trajectory_generator_circ.cpp | 6 +- .../src/trajectory_generator_lin.cpp | 6 +- .../src/trajectory_generator_ptp.cpp | 6 +- .../stomp_moveit/conversion_functions.hpp | 20 +-- .../include/stomp_moveit/cost_functions.hpp | 24 +-- .../include/stomp_moveit/filter_functions.hpp | 4 +- .../include/stomp_moveit/noise_generators.hpp | 2 +- .../stomp_moveit/trajectory_visualization.hpp | 18 +-- .../src/stomp_moveit_planning_context.cpp | 22 +-- ...low_joint_trajectory_controller_handle.cpp | 15 +- .../benchmarks/src/BenchmarkExecutor.cpp | 2 +- .../move_group/src/move_group_capability.cpp | 2 +- .../src/occupancy_map_monitor.cpp | 2 +- ...ccupancy_map_monitor_middleware_handle.cpp | 2 +- .../depth_image_octomap_updater.h | 1 - .../src/depth_image_octomap_updater.cpp | 14 +- .../include/moveit/mesh_filter/gl_renderer.h | 6 +- .../mesh_filter/src/gl_renderer.cpp | 28 ++-- .../pointcloud_octomap_updater.h | 3 +- .../src/pointcloud_octomap_updater.cpp | 12 +- .../plan_execution/src/plan_execution.cpp | 2 +- .../solution_selection_functions.hpp | 2 +- .../src/solution_selection_functions.cpp | 4 +- .../src/current_state_monitor.cpp | 2 +- .../src/move_group_interface.cpp | 12 +- .../src/motion_planning_frame_objects.cpp | 6 +- .../src/motion_planning_param_widget.cpp | 8 +- 67 files changed, 355 insertions(+), 349 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index 007ab813e8..cf51a5d441 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -18,6 +18,7 @@ Checks: '-*, readability-simplify-boolean-expr, readability-container-size-empty, readability-identifier-naming, + readability-static-definition-in-anonymous-namespace, ' HeaderFilterRegex: '' AnalyzeTemporaryDtors: false @@ -37,6 +38,9 @@ CheckOptions: value: CamelCase - key: readability-identifier-naming.UnionCase value: CamelCase + # function names + - key: readability-identifier-naming.FunctionCase + value: camelBack # method names - key: readability-identifier-naming.MethodCase value: camelBack @@ -50,8 +54,12 @@ CheckOptions: # const static or global variables are UPPER_CASE - key: readability-identifier-naming.EnumConstantCase value: UPPER_CASE + - key: readability-identifier-naming.StaticVariableCasePrefix + value: 's_' - key: readability-identifier-naming.StaticConstantCase value: UPPER_CASE - key: readability-identifier-naming.GlobalConstantCase value: UPPER_CASE + - key: readability-identifier-naming.ClassConstantCase + value: UPPER_CASE ... diff --git a/moveit_core/collision_detection/test/test_world.cpp b/moveit_core/collision_detection/test/test_world.cpp index 658e7960ff..f2d2c09227 100644 --- a/moveit_core/collision_detection/test/test_world.cpp +++ b/moveit_core/collision_detection/test/test_world.cpp @@ -224,7 +224,7 @@ struct TestAction }; /* notification callback */ -static void TrackChangesNotify(TestAction& ta, const World::ObjectConstPtr& obj, World::Action action) +static void trackChangesNotify(TestAction& ta, const World::ObjectConstPtr& obj, World::Action action) { ta.obj_ = *obj; ta.action_ = action; @@ -238,7 +238,7 @@ TEST(World, TrackChanges) TestAction ta; World::ObserverHandle observer_ta; observer_ta = world.addObserver([&ta](const World::ObjectConstPtr& object, World::Action action) { - return TrackChangesNotify(ta, object, action); + return trackChangesNotify(ta, object, action); }); // Create some shapes @@ -271,7 +271,7 @@ TEST(World, TrackChanges) TestAction ta2; World::ObserverHandle observer_ta2; observer_ta2 = world.addObserver([&ta2](const World::ObjectConstPtr& object, World::Action action) { - return TrackChangesNotify(ta2, object, action); + return trackChangesNotify(ta2, object, action); }); world.addToObject("obj2", cyl, Eigen::Isometry3d::Identity()); @@ -311,7 +311,7 @@ TEST(World, TrackChanges) TestAction ta3; World::ObserverHandle observer_ta3; observer_ta3 = world.addObserver([&ta3](const World::ObjectConstPtr& object, World::Action action) { - return TrackChangesNotify(ta3, object, action); + return trackChangesNotify(ta3, object, action); }); bool rm_good = world.removeShapeFromObject("obj2", cyl); @@ -379,7 +379,7 @@ TEST(World, ObjectPoseAndSubframes) TestAction ta; World::ObserverHandle observer_ta; observer_ta = world.addObserver([&ta](const World::ObjectConstPtr& object, World::Action action) { - return TrackChangesNotify(ta, object, action); + return trackChangesNotify(ta, object, action); }); // Create shapes diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp index 555aded8ee..658a84c119 100644 --- a/moveit_core/collision_detection_fcl/src/collision_common.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp @@ -709,7 +709,7 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void * * The returned cache is a quasi-singleton for each thread as it is created \e thread_local. */ template -FCLShapeCache& GetShapeCache() +FCLShapeCache& getShapeCache() { /* The cache is created thread_local, that is each thread calling * this quasi-singleton function will get its own instance. Once @@ -735,7 +735,7 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, using ShapeKey = shapes::ShapeConstWeakPtr; using ShapeMap = std::map>; - FCLShapeCache& cache = GetShapeCache(); + FCLShapeCache& cache = getShapeCache(); shapes::ShapeConstWeakPtr wptr(shape); { @@ -767,7 +767,7 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, if (std::is_same::value) { // get the cache that corresponds to objects; maybe this attached object used to be a world object - FCLShapeCache& othercache = GetShapeCache(); + FCLShapeCache& othercache = getShapeCache(); // attached bodies could be just moved from the environment. auto cache_it = othercache.map_.find(wptr); @@ -800,7 +800,7 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, if (std::is_same::value) { // get the cache that corresponds to objects; maybe this attached object used to be a world object - FCLShapeCache& othercache = GetShapeCache(); + FCLShapeCache& othercache = getShapeCache(); // attached bodies could be just moved from the environment. auto cache_it = othercache.map_.find(wptr); @@ -966,11 +966,11 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, void cleanCollisionGeometryCache() { - FCLShapeCache& cache1 = GetShapeCache(); + FCLShapeCache& cache1 = getShapeCache(); { cache1.bumpUseCount(true); } - FCLShapeCache& cache2 = GetShapeCache(); + FCLShapeCache& cache2 = getShapeCache(); { cache2.bumpUseCount(true); } diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp index eec415406c..4552a8ad8e 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp @@ -105,7 +105,7 @@ void PR2ArmIKSolver::updateInternalDataStructures() int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_in, KDL::JntArray& q_out) { const bool verbose = false; - Eigen::Isometry3f b = KDLToEigenMatrix(p_in); + Eigen::Isometry3f b = kdlToEigenMatrix(p_in); std::vector > solution_ik; if (free_angle_ == 0) { @@ -221,7 +221,7 @@ bool getKDLChain(const urdf::ModelInterface& model, const std::string& root_name return true; } -Eigen::Isometry3f KDLToEigenMatrix(const KDL::Frame& p) +Eigen::Isometry3f kdlToEigenMatrix(const KDL::Frame& p) { Eigen::Isometry3f b = Eigen::Isometry3f::Identity(); for (int i = 0; i < 3; ++i) diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h index 1d231fc932..45c73fe87b 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h @@ -115,7 +115,7 @@ class PR2ArmIKSolver : public KDL::ChainIkSolverPos std::string root_frame_name_; }; -Eigen::Isometry3f KDLToEigenMatrix(const KDL::Frame& p); +Eigen::Isometry3f kdlToEigenMatrix(const KDL::Frame& p); double computeEuclideanDistance(const std::vector& array_1, const KDL::JntArray& array_2); void getKDLChainInfo(const KDL::Chain& chain, moveit_msgs::msg::KinematicSolverInfo& chain_info); diff --git a/moveit_core/distance_field/test/test_distance_field.cpp b/moveit_core/distance_field/test/test_distance_field.cpp index 87a3f0d973..5792eaa336 100644 --- a/moveit_core/distance_field/test/test_distance_field.cpp +++ b/moveit_core/distance_field/test/test_distance_field.cpp @@ -59,7 +59,7 @@ 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); -int dist_sq(int x, int y, int z) +int distanceSequence(int x, int y, int z) { return x * x + y * y + z * z; } @@ -295,8 +295,8 @@ unsigned int countLeafNodes(const octomap::OcTree& octree) } // points should contain all occupied points -void check_distance_field(const PropagationDistanceField& df, const EigenSTL::vector_Vector3d& points, int numX, - int numY, int numZ, bool do_negs) +void checkDistanceField(const PropagationDistanceField& df, const EigenSTL::vector_Vector3d& points, int numX, int numY, + int numZ, bool do_negs) { EigenSTL::vector_Vector3i points_ind(points.size()); for (unsigned int i = 0; i < points.size(); ++i) @@ -374,7 +374,7 @@ TEST(TestPropagationDistanceField, TestAddRemovePoints) // std::cout << "One removal, one addition" << '\n'; // print(df, numX, numY, numZ); // printNeg(df, numX, numY, numZ); - check_distance_field(df, points, num_x, num_y, num_z, false); + checkDistanceField(df, points, num_x, num_y, num_z, false); // Remove points.clear(); @@ -382,7 +382,7 @@ TEST(TestPropagationDistanceField, TestAddRemovePoints) df.removePointsFromField(points); points.clear(); points.push_back(POINT3); - check_distance_field(df, points, num_x, num_y, num_z, false); + checkDistanceField(df, points, num_x, num_y, num_z, false); // now testing gradient calls df.reset(); @@ -455,7 +455,7 @@ TEST(TestSignedPropagationDistanceField, TestSignedAddRemovePoints) // print(df, numX, numY, numZ); // TODO - check initial values - // EXPECT_EQ( df.getCell(0,0,0).distance_square_, max_dist_sq_in_voxels ); + // EXPECT_EQ( df.getCell(0,0,0).distance_square_, max_distanceSequence_in_voxels ); // Add points to the grid double lwx, lwy, lwz; @@ -627,7 +627,7 @@ TEST(TestSignedPropagationDistanceField, TestShape) EigenSTL::vector_Vector3d point_vec; findInternalPointsConvex(*body, RESOLUTION, point_vec); delete body; - check_distance_field(df, point_vec, num_x, num_y, num_z, true); + checkDistanceField(df, point_vec, num_x, num_y, num_z, true); // std::cout << "Shape pos "<< '\n'; // print(df, numX, numY, numZ); @@ -644,12 +644,12 @@ TEST(TestSignedPropagationDistanceField, TestShape) 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()); - check_distance_field(df, point_vec_union, num_x, num_y, num_z, true); + checkDistanceField(df, point_vec_union, num_x, num_y, num_z, true); // should get rid of old pose df.moveShapeInField(&sphere, p, np); - check_distance_field(df, point_vec_2, num_x, num_y, num_z, true); + checkDistanceField(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); diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 641e0171b3..68ac2be507 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -89,7 +89,7 @@ static double normalizeAbsoluteAngle(const double angle) */ template std::tuple::Scalar, 3, 1>, bool> -CalcEulerAngles(const Eigen::MatrixBase& R) +calcEulerAngles(const Eigen::MatrixBase& R) { using std::atan2; using std::sqrt; @@ -721,11 +721,11 @@ ConstraintEvaluationResult OrientationConstraint::decide(const moveit::core::Rob Eigen::Vector3d xyz_rotation; if (parameterization_type_ == moveit_msgs::msg::OrientationConstraint::XYZ_EULER_ANGLES) { - euler_angles_error = CalcEulerAngles(diff.linear()); + euler_angles_error = calcEulerAngles(diff.linear()); // Converting from a rotation matrix to intrinsic XYZ Euler angles has 2 singularities: // pitch ~= pi/2 ==> roll + yaw = theta // pitch ~= -pi/2 ==> roll - yaw = theta - // in those cases CalcEulerAngles will set roll (xyz_rotation(0)) to theta and yaw (xyz_rotation(2)) to zero, so for + // in those cases calcEulerAngles will set roll (xyz_rotation(0)) to theta and yaw (xyz_rotation(2)) to zero, so for // us to be able to capture yaw tolerance violations we do the following: If theta violates the absolute yaw // tolerance we think of it as a pure yaw rotation and set roll to zero. xyz_rotation = std::get(euler_angles_error); diff --git a/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp b/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp index 25c91af791..ea0e49608d 100644 --- a/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp +++ b/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp @@ -126,14 +126,14 @@ class FloatingJointRobot : public testing::Test }; /** Helper function to create a quaternion from Euler angles. **/ -inline Eigen::Quaterniond xyz_intrinsix_to_quat(double rx, double ry, double rz) +inline Eigen::Quaterniond xyzIntrinsixToQuaternion(double rx, double ry, double rz) { return Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()); } /** Helper function to create a quaternion from a rotation vector. **/ -inline Eigen::Quaterniond rotation_vector_to_quat(double rx, double ry, double rz) +inline Eigen::Quaterniond rotationVectorToQuaternion(double rx, double ry, double rz) { Eigen::Vector3d v{ rx, ry, rz }; Eigen::Matrix3d m{ Eigen::AngleAxisd(v.norm(), v.normalized()) }; @@ -415,16 +415,16 @@ TEST_F(FloatingJointRobot, OrientationConstraintsParameterization) EXPECT_TRUE(oc_rotvec.decide(robot_state, true).satisfied); // some trivial test cases - setRobotEndEffectorOrientation(robot_state, xyz_intrinsix_to_quat(0.1, 0.2, -0.3)); + setRobotEndEffectorOrientation(robot_state, xyzIntrinsixToQuaternion(0.1, 0.2, -0.3)); EXPECT_TRUE(oc_euler.decide(robot_state).satisfied); - setRobotEndEffectorOrientation(robot_state, xyz_intrinsix_to_quat(0.1, 0.2, -0.6)); + setRobotEndEffectorOrientation(robot_state, xyzIntrinsixToQuaternion(0.1, 0.2, -0.6)); EXPECT_FALSE(oc_euler.decide(robot_state).satisfied); - setRobotEndEffectorOrientation(robot_state, rotation_vector_to_quat(0.1, 0.2, -0.3)); + setRobotEndEffectorOrientation(robot_state, rotationVectorToQuaternion(0.1, 0.2, -0.3)); EXPECT_TRUE(oc_rotvec.decide(robot_state).satisfied); - setRobotEndEffectorOrientation(robot_state, rotation_vector_to_quat(0.1, 0.2, -0.6)); + setRobotEndEffectorOrientation(robot_state, rotationVectorToQuaternion(0.1, 0.2, -0.6)); EXPECT_FALSE(oc_rotvec.decide(robot_state).satisfied); // more extensive testing using the test data hardcoded at the top of this file @@ -457,20 +457,20 @@ TEST_F(FloatingJointRobot, OrientationConstraintsParameterization) robot_state.update(); ocm.parameterization = moveit_msgs::msg::OrientationConstraint::XYZ_EULER_ANGLES; - ocm.orientation = tf2::toMsg(xyz_intrinsix_to_quat(0.1, 0.2, -0.3)); + ocm.orientation = tf2::toMsg(xyzIntrinsixToQuaternion(0.1, 0.2, -0.3)); EXPECT_TRUE(oc_euler.configure(ocm, tf)); EXPECT_TRUE(oc_euler.decide(robot_state).satisfied); - ocm.orientation = tf2::toMsg(xyz_intrinsix_to_quat(0.1, 0.2, -0.6)); + ocm.orientation = tf2::toMsg(xyzIntrinsixToQuaternion(0.1, 0.2, -0.6)); EXPECT_TRUE(oc_euler.configure(ocm, tf)); EXPECT_FALSE(oc_euler.decide(robot_state).satisfied); ocm.parameterization = moveit_msgs::msg::OrientationConstraint::ROTATION_VECTOR; - ocm.orientation = tf2::toMsg(rotation_vector_to_quat(0.1, 0.2, -0.3)); + ocm.orientation = tf2::toMsg(rotationVectorToQuaternion(0.1, 0.2, -0.3)); EXPECT_TRUE(oc_rotvec.configure(ocm, tf)); EXPECT_TRUE(oc_rotvec.decide(robot_state).satisfied); - ocm.orientation = tf2::toMsg(rotation_vector_to_quat(0.1, 0.2, -0.6)); + ocm.orientation = tf2::toMsg(rotationVectorToQuaternion(0.1, 0.2, -0.6)); EXPECT_TRUE(oc_rotvec.configure(ocm, tf)); EXPECT_FALSE(oc_rotvec.decide(robot_state).satisfied); } diff --git a/moveit_core/planning_interface/src/planning_interface.cpp b/moveit_core/planning_interface/src/planning_interface.cpp index d64ccc7c69..c3e594c850 100644 --- a/moveit_core/planning_interface/src/planning_interface.cpp +++ b/moveit_core/planning_interface/src/planning_interface.cpp @@ -53,9 +53,9 @@ struct ActiveContexts std::set contexts_; }; -static ActiveContexts& getActiveContexts() +ActiveContexts& getActiveContexts() { - static ActiveContexts ac; + ActiveContexts ac; return ac; } } // namespace diff --git a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp index c5ddc0841f..5a126ae784 100644 --- a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp +++ b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp @@ -72,7 +72,7 @@ bool callAdapter(const PlanningRequestAdapter& adapter, const PlanningRequestAda std::swap(res.added_path_index, added_path_index); bool result = adapter.adaptAndPlan(planner, planning_scene, req, res); std::swap(res.added_path_index, added_path_index); - RCLCPP_DEBUG_STREAM(LOGGER, adapter.getDescription() << ": " << moveit::core::error_code_to_string(res.error_code)); + RCLCPP_DEBUG_STREAM(LOGGER, adapter.getDescription() << ": " << moveit::core::errorCodeToString(res.error_code)); return result; } catch (std::exception& ex) diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp index bb841e3b7d..4bd5c25ce9 100644 --- a/moveit_core/planning_scene/test/test_planning_scene.cpp +++ b/moveit_core/planning_scene/test/test_planning_scene.cpp @@ -392,10 +392,10 @@ TEST_P(CollisionDetectorTests, ClearDiff) } // Returns a planning scene diff message -moveit_msgs::msg::PlanningScene create_planning_scene_diff(const planning_scene::PlanningScene& ps, - const std::string& object_name, const int8_t operation, - const bool attach_object = false, - const bool create_object = true) +moveit_msgs::msg::PlanningScene createPlanningSceneDiff(const planning_scene::PlanningScene& ps, + const std::string& object_name, const int8_t operation, + const bool attach_object = false, + const bool create_object = true) { // Helper function to create an object for RobotStateDiffBug auto add_object = [](const std::string& object_name, const int8_t operation) { @@ -438,7 +438,7 @@ moveit_msgs::msg::PlanningScene create_planning_scene_diff(const planning_scene: } // Returns collision objects names sorted alphabetically -std::set get_collision_objects_names(const planning_scene::PlanningScene& ps) +std::set getCollisionObjectsNames(const planning_scene::PlanningScene& ps) { std::vector collision_objects; ps.getCollisionObjectMsgs(collision_objects); @@ -449,7 +449,7 @@ std::set get_collision_objects_names(const planning_scene::Planning } // Returns attached collision objects names sorted alphabetically -std::set get_attached_collision_objects_names(const planning_scene::PlanningScene& ps) +std::set getAttachedCollisionObjectsNames(const planning_scene::PlanningScene& ps) { std::vector collision_objects; ps.getAttachedCollisionObjectMsgs(collision_objects); @@ -467,51 +467,51 @@ TEST(PlanningScene, RobotStateDiffBug) // Adding collision objects incrementally { - const auto ps1 = create_planning_scene_diff(*ps, "object1", moveit_msgs::msg::CollisionObject::ADD); - const auto ps2 = create_planning_scene_diff(*ps, "object2", moveit_msgs::msg::CollisionObject::ADD); + const auto ps1 = createPlanningSceneDiff(*ps, "object1", moveit_msgs::msg::CollisionObject::ADD); + const auto ps2 = createPlanningSceneDiff(*ps, "object2", moveit_msgs::msg::CollisionObject::ADD); ps->usePlanningSceneMsg(ps1); ps->usePlanningSceneMsg(ps2); - EXPECT_EQ(get_collision_objects_names(*ps), (std::set{ "object1", "object2" })); + EXPECT_EQ(getCollisionObjectsNames(*ps), (std::set{ "object1", "object2" })); } // Removing a collision object { - const auto ps1 = create_planning_scene_diff(*ps, "object2", moveit_msgs::msg::CollisionObject::REMOVE); + const auto ps1 = createPlanningSceneDiff(*ps, "object2", moveit_msgs::msg::CollisionObject::REMOVE); ps->usePlanningSceneMsg(ps1); - EXPECT_EQ(get_collision_objects_names(*ps), (std::set{ "object1" })); + EXPECT_EQ(getCollisionObjectsNames(*ps), (std::set{ "object1" })); } // Adding attached collision objects incrementally ps = std::make_shared(urdf_model, srdf_model); { - const auto ps1 = create_planning_scene_diff(*ps, "object1", moveit_msgs::msg::CollisionObject::ADD, true); - const auto ps2 = create_planning_scene_diff(*ps, "object2", moveit_msgs::msg::CollisionObject::ADD, true); + const auto ps1 = createPlanningSceneDiff(*ps, "object1", moveit_msgs::msg::CollisionObject::ADD, true); + const auto ps2 = createPlanningSceneDiff(*ps, "object2", moveit_msgs::msg::CollisionObject::ADD, true); ps->usePlanningSceneMsg(ps1); ps->usePlanningSceneMsg(ps2); - EXPECT_TRUE(get_collision_objects_names(*ps).empty()); - EXPECT_EQ(get_attached_collision_objects_names(*ps), (std::set{ "object1", "object2" })); + EXPECT_TRUE(getCollisionObjectsNames(*ps).empty()); + EXPECT_EQ(getAttachedCollisionObjectsNames(*ps), (std::set{ "object1", "object2" })); } // Removing an attached collision object { - const auto ps1 = create_planning_scene_diff(*ps, "object2", moveit_msgs::msg::CollisionObject::REMOVE, true); + const auto ps1 = createPlanningSceneDiff(*ps, "object2", moveit_msgs::msg::CollisionObject::REMOVE, true); ps->usePlanningSceneMsg(ps1); - EXPECT_EQ(get_collision_objects_names(*ps), (std::set{ "object2" })); - EXPECT_EQ(get_attached_collision_objects_names(*ps), (std::set{ "object1" })); + EXPECT_EQ(getCollisionObjectsNames(*ps), (std::set{ "object2" })); + EXPECT_EQ(getAttachedCollisionObjectsNames(*ps), (std::set{ "object1" })); } // Turn an existing collision object into an attached object { - const auto ps1 = create_planning_scene_diff(*ps, "object2", moveit_msgs::msg::CollisionObject::ADD, true, false); + const auto ps1 = createPlanningSceneDiff(*ps, "object2", moveit_msgs::msg::CollisionObject::ADD, true, false); ps->usePlanningSceneMsg(ps1); - EXPECT_TRUE(get_collision_objects_names(*ps).empty()); - EXPECT_EQ(get_attached_collision_objects_names(*ps), (std::set{ "object1", "object2" })); + EXPECT_TRUE(getCollisionObjectsNames(*ps).empty()); + EXPECT_EQ(getAttachedCollisionObjectsNames(*ps), (std::set{ "object1", "object2" })); } // Removing an attached collision object completely @@ -530,8 +530,8 @@ TEST(PlanningScene, RobotStateDiffBug) ps1->getPlanningSceneDiffMsg(msg); ps->usePlanningSceneMsg(msg); - EXPECT_TRUE(get_collision_objects_names(*ps).empty()); - EXPECT_EQ(get_attached_collision_objects_names(*ps), (std::set{ "object1" })); + EXPECT_TRUE(getCollisionObjectsNames(*ps).empty()); + EXPECT_EQ(getAttachedCollisionObjectsNames(*ps), (std::set{ "object1" })); } } diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index a9da7f9aa6..9779b0585a 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -51,7 +51,7 @@ namespace core { namespace { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_robot_model.joint_model_group"); +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_robot_model.joint_model_group"); // check if a parent or ancestor of joint is included in this group bool includesParent(const JointModel* joint, const JointModelGroup* group) diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 4fa903a626..b68ac8ba49 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -885,7 +885,7 @@ JointModel* RobotModel::buildRecursive(LinkModel* parent, const urdf::Link* urdf namespace { // construct bounds for 1DOF joint -static inline VariableBounds jointBoundsFromURDF(const urdf::Joint* urdf_joint) +inline VariableBounds jointBoundsFromURDF(const urdf::Joint* urdf_joint) { VariableBounds b; if (urdf_joint->safety) @@ -1155,7 +1155,7 @@ JointModel* RobotModel::constructJointModel(const urdf::Link* child_link, const namespace { -static inline Eigen::Isometry3d urdfPose2Isometry3d(const urdf::Pose& pose) +inline Eigen::Isometry3d urdfPose2Isometry3d(const urdf::Pose& pose) { Eigen::Quaterniond q(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z); Eigen::Isometry3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z) * q); diff --git a/moveit_core/robot_state/src/conversions.cpp b/moveit_core/robot_state/src/conversions.cpp index 0e3e0d2e4b..c1a222de51 100644 --- a/moveit_core/robot_state/src/conversions.cpp +++ b/moveit_core/robot_state/src/conversions.cpp @@ -35,6 +35,7 @@ /* Author: Ioan Sucan, Dave Coleman */ +#include #include #include #include @@ -53,9 +54,9 @@ namespace core namespace { // Logger -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_robot_state.conversions"); +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_robot_state.conversions"); -static bool _jointStateToRobotState(const sensor_msgs::msg::JointState& joint_state, RobotState& state) +bool jointStateToRobotStateImpl(const sensor_msgs::msg::JointState& joint_state, RobotState& state) { if (joint_state.name.size() != joint_state.position.size()) { @@ -69,8 +70,7 @@ static bool _jointStateToRobotState(const sensor_msgs::msg::JointState& joint_st return true; } -static bool _multiDOFJointsToRobotState(const sensor_msgs::msg::MultiDOFJointState& mjs, RobotState& state, - const Transforms* tf) +bool multiDofJointsToRobotState(const sensor_msgs::msg::MultiDOFJointState& mjs, RobotState& state, const Transforms* tf) { std::size_t nj = mjs.joint_names.size(); if (nj != mjs.transforms.size()) @@ -136,7 +136,7 @@ static bool _multiDOFJointsToRobotState(const sensor_msgs::msg::MultiDOFJointSta return !error; } -static inline void _robotStateToMultiDOFJointState(const RobotState& state, sensor_msgs::msg::MultiDOFJointState& mjs) +void robotStateToMultiDofJointState(const RobotState& state, sensor_msgs::msg::MultiDOFJointState& mjs) { const std::vector& js = state.getRobotModel()->getMultiDOFJointModels(); mjs.joint_names.clear(); @@ -195,7 +195,7 @@ class ShapeVisitorAddToCollisionObject : public boost::static_visitor const geometry_msgs::msg::Pose* pose_; }; -static void _attachedBodyToMsg(const AttachedBody& attached_body, moveit_msgs::msg::AttachedCollisionObject& aco) +void attachedBodyToMsg(const AttachedBody& attached_body, moveit_msgs::msg::AttachedCollisionObject& aco) { aco.link_name = attached_body.getAttachedLinkName(); aco.detach_posture = attached_body.getDetachPosture(); @@ -238,8 +238,7 @@ static void _attachedBodyToMsg(const AttachedBody& attached_body, moveit_msgs::m } } -static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::AttachedCollisionObject& aco, - RobotState& state) +void msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::AttachedCollisionObject& aco, RobotState& state) { if (aco.object.operation == moveit_msgs::msg::CollisionObject::ADD) { @@ -363,8 +362,8 @@ static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::Att RCLCPP_ERROR(LOGGER, "Unknown collision object operation: %d", aco.object.operation); } -static bool _robotStateMsgToRobotStateHelper(const Transforms* tf, const moveit_msgs::msg::RobotState& robot_state, - RobotState& state, bool copy_attached_bodies) +bool robotStateMsgToRobotStateHelper(const Transforms* tf, const moveit_msgs::msg::RobotState& robot_state, + RobotState& state, bool copy_attached_bodies) { bool valid; const moveit_msgs::msg::RobotState& rs = robot_state; @@ -375,8 +374,8 @@ static bool _robotStateMsgToRobotStateHelper(const Transforms* tf, const moveit_ return false; } - bool result1 = _jointStateToRobotState(robot_state.joint_state, state); - bool result2 = _multiDOFJointsToRobotState(robot_state.multi_dof_joint_state, state, tf); + bool result1 = jointStateToRobotStateImpl(robot_state.joint_state, state); + bool result2 = multiDofJointsToRobotState(robot_state.multi_dof_joint_state, state, tf); valid = result1 || result2; if (valid && copy_attached_bodies) @@ -385,7 +384,7 @@ static bool _robotStateMsgToRobotStateHelper(const Transforms* tf, const moveit_ state.clearAttachedBodies(); for (const moveit_msgs::msg::AttachedCollisionObject& attached_collision_object : robot_state.attached_collision_objects) - _msgToAttachedBody(tf, attached_collision_object, state); + msgToAttachedBody(tf, attached_collision_object, state); } return valid; @@ -400,7 +399,7 @@ static bool _robotStateMsgToRobotStateHelper(const Transforms* tf, const moveit_ bool jointStateToRobotState(const sensor_msgs::msg::JointState& joint_state, RobotState& state) { - bool result = _jointStateToRobotState(joint_state, state); + bool result = jointStateToRobotStateImpl(joint_state, state); state.update(); return result; } @@ -408,7 +407,7 @@ bool jointStateToRobotState(const sensor_msgs::msg::JointState& joint_state, Rob bool robotStateMsgToRobotState(const moveit_msgs::msg::RobotState& robot_state, RobotState& state, bool copy_attached_bodies) { - bool result = _robotStateMsgToRobotStateHelper(nullptr, robot_state, state, copy_attached_bodies); + bool result = robotStateMsgToRobotStateHelper(nullptr, robot_state, state, copy_attached_bodies); state.update(); return result; } @@ -416,7 +415,7 @@ bool robotStateMsgToRobotState(const moveit_msgs::msg::RobotState& robot_state, bool robotStateMsgToRobotState(const Transforms& tf, const moveit_msgs::msg::RobotState& robot_state, RobotState& state, bool copy_attached_bodies) { - bool result = _robotStateMsgToRobotStateHelper(&tf, robot_state, state, copy_attached_bodies); + bool result = robotStateMsgToRobotStateHelper(&tf, robot_state, state, copy_attached_bodies); state.update(); return result; } @@ -426,7 +425,7 @@ void robotStateToRobotStateMsg(const RobotState& state, moveit_msgs::msg::RobotS { robot_state.is_diff = false; robotStateToJointStateMsg(state, robot_state.joint_state); - _robotStateToMultiDOFJointState(state, robot_state.multi_dof_joint_state); + robotStateToMultiDofJointState(state, robot_state.multi_dof_joint_state); if (copy_attached_bodies) { @@ -442,7 +441,7 @@ void attachedBodiesToAttachedCollisionObjectMsgs( { attached_collision_objs.resize(attached_bodies.size()); for (std::size_t i = 0; i < attached_bodies.size(); ++i) - _attachedBodyToMsg(*attached_bodies[i], attached_collision_objs[i]); + attachedBodyToMsg(*attached_bodies[i], attached_collision_objs[i]); } void robotStateToJointStateMsg(const RobotState& state, sensor_msgs::msg::JointState& joint_state) diff --git a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp index 2245dc129d..d967b93399 100644 --- a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp +++ b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp @@ -53,7 +53,7 @@ class SimpleRobot : public testing::Test builder.addChain("a->b", "continuous"); builder.addChain("b->c", "prismatic"); builder.addGroupChain("a", "c", "group"); - robot_model_ = builder.build(); + robot_model = builder.build(); } void TearDown() override @@ -61,18 +61,18 @@ class SimpleRobot : public testing::Test } protected: - RobotModelConstPtr robot_model_; + RobotModelConstPtr robot_model; static std::size_t generateTestTraj(std::vector>& traj, - const RobotModelConstPtr& robot_model_); + const RobotModelConstPtr& robot_model); }; std::size_t SimpleRobot::generateTestTraj(std::vector>& traj, - const RobotModelConstPtr& robot_model_) + const RobotModelConstPtr& robot_model) { traj.clear(); - auto robot_state = std::make_shared(robot_model_); + auto robot_state = std::make_shared(robot_model); robot_state->setToDefaultValues(); double ja, jc; @@ -116,7 +116,7 @@ TEST_F(SimpleRobot, testGenerateTrajectory) const std::size_t expected_full_traj_len = 7; // Generate a test trajectory - std::size_t full_traj_len = generateTestTraj(traj, robot_model_); + std::size_t full_traj_len = generateTestTraj(traj, robot_model); // Test the generateTestTraj still generates a trajectory of length 7 EXPECT_EQ(full_traj_len, expected_full_traj_len); // full traj should be 7 waypoints long @@ -124,7 +124,7 @@ TEST_F(SimpleRobot, testGenerateTrajectory) TEST_F(SimpleRobot, checkAbsoluteJointSpaceJump) { - const JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("group"); + const JointModelGroup* joint_model_group = robot_model->getJointModelGroup("group"); std::vector> traj; // A revolute joint jumps 1.01 at the 5th waypoint and a prismatic joint jumps 1.01 at the 6th waypoint @@ -132,7 +132,7 @@ TEST_F(SimpleRobot, checkAbsoluteJointSpaceJump) const std::size_t expected_prismatic_jump_traj_len = 5; // Pre-compute expected results for tests - std::size_t full_traj_len = generateTestTraj(traj, robot_model_); + std::size_t full_traj_len = generateTestTraj(traj, robot_model); const double expected_revolute_jump_fraction = static_cast(expected_revolute_jump_traj_len) / static_cast(full_traj_len); const double expected_prismatic_jump_fraction = @@ -147,25 +147,25 @@ TEST_F(SimpleRobot, checkAbsoluteJointSpaceJump) EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01); // Indirect call using checkJointSpaceJumps - generateTestTraj(traj, robot_model_); + generateTestTraj(traj, robot_model); fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(1.0, 1.0)); EXPECT_EQ(expected_revolute_jump_traj_len, traj.size()); // traj should be cut before the revolute jump EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01); // Test revolute joints - generateTestTraj(traj, robot_model_); + generateTestTraj(traj, robot_model); fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(1.0, 0.0)); EXPECT_EQ(expected_revolute_jump_traj_len, traj.size()); // traj should be cut before the revolute jump EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01); // Test prismatic joints - generateTestTraj(traj, robot_model_); + generateTestTraj(traj, robot_model); fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(0.0, 1.0)); EXPECT_EQ(expected_prismatic_jump_traj_len, traj.size()); // traj should be cut before the prismatic jump EXPECT_NEAR(expected_prismatic_jump_fraction, fraction, 0.01); // Ignore all absolute jumps - generateTestTraj(traj, robot_model_); + generateTestTraj(traj, robot_model); fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(0.0, 0.0)); EXPECT_EQ(full_traj_len, traj.size()); // traj should not be cut EXPECT_NEAR(1.0, fraction, 0.01); @@ -173,14 +173,14 @@ TEST_F(SimpleRobot, checkAbsoluteJointSpaceJump) TEST_F(SimpleRobot, checkRelativeJointSpaceJump) { - const JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("group"); + const JointModelGroup* joint_model_group = robot_model->getJointModelGroup("group"); std::vector> traj; // The first large jump of 1.01 occurs at the 5th waypoint so the test should trim the trajectory to length 4 const std::size_t expected_relative_jump_traj_len = 4; // Pre-compute expected results for tests - std::size_t full_traj_len = generateTestTraj(traj, robot_model_); + std::size_t full_traj_len = generateTestTraj(traj, robot_model); const double expected_relative_jump_fraction = static_cast(expected_relative_jump_traj_len) / static_cast(full_traj_len); @@ -193,13 +193,13 @@ TEST_F(SimpleRobot, checkRelativeJointSpaceJump) EXPECT_NEAR(expected_relative_jump_fraction, fraction, 0.01); // Indirect call of relative version using checkJointSpaceJumps - generateTestTraj(traj, robot_model_); + generateTestTraj(traj, robot_model); fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(2.97)); EXPECT_EQ(expected_relative_jump_traj_len, traj.size()); // traj should be cut before the first jump of 1.01 EXPECT_NEAR(expected_relative_jump_fraction, fraction, 0.01); // Trajectory should not be cut: 1.01 < 2.98 * (0.01 * 2 + 1.01 * 2)/6. - generateTestTraj(traj, robot_model_); + generateTestTraj(traj, robot_model); fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(2.98)); EXPECT_EQ(full_traj_len, traj.size()); // traj should not be cut EXPECT_NEAR(1.0, fraction, 0.01); @@ -210,30 +210,30 @@ class PandaRobot : public testing::Test protected: static void SetUpTestSuite() // setup resources shared between tests { - robot_model_ = loadTestingRobotModel("panda"); - jmg_ = robot_model_->getJointModelGroup("panda_arm"); - link_ = robot_model_->getLinkModel("panda_link8"); - ASSERT_TRUE(link_); - node_ = rclcpp::Node::make_shared("test_cartesian_interpolator"); - loadIKPluginForGroup(node_, jmg_, "panda_link0", link_->getName()); + robot_model = loadTestingRobotModel("panda"); + jmg = robot_model->getJointModelGroup("panda_arm"); + link = robot_model->getLinkModel("panda_link8"); + ASSERT_TRUE(link); + node = rclcpp::Node::make_shared("test_cartesian_interpolator"); + loadIKPluginForGroup(node, jmg, "panda_link0", link->getName()); } static void TearDownTestSuite() { - robot_model_.reset(); + robot_model.reset(); } void SetUp() override { - start_state_ = std::make_shared(robot_model_); - ASSERT_TRUE(start_state_->setToDefaultValues(jmg_, "ready")); - start_pose_ = start_state_->getGlobalLinkTransform(link_); + start_state = std::make_shared(robot_model); + ASSERT_TRUE(start_state->setToDefaultValues(jmg, "ready")); + start_pose = start_state->getGlobalLinkTransform(link); } double computeCartesianPath(std::vector>& result, const Eigen::Vector3d& translation, bool global) { - return CartesianInterpolator::computeCartesianPath(start_state_.get(), jmg_, result, link_, translation, global, + return CartesianInterpolator::computeCartesianPath(start_state.get(), jmg, result, link, translation, global, MaxEEFStep(0.1), JumpThreshold(), GroupStateValidityCallbackFn(), kinematics::KinematicsQueryOptions()); } @@ -241,22 +241,22 @@ class PandaRobot : public testing::Test double computeCartesianPath(std::vector>& result, const Eigen::Isometry3d& target, bool global, const Eigen::Isometry3d& offset = Eigen::Isometry3d::Identity()) { - return CartesianInterpolator::computeCartesianPath(start_state_.get(), jmg_, result, link_, target, global, + return CartesianInterpolator::computeCartesianPath(start_state.get(), jmg, result, link, target, global, MaxEEFStep(0.1), JumpThreshold(), GroupStateValidityCallbackFn(), kinematics::KinematicsQueryOptions(), kinematics::KinematicsBase::IKCostFn(), offset); } protected: - static RobotModelPtr robot_model_; - static JointModelGroup* jmg_; - static const LinkModel* link_; - static rclcpp::Node::SharedPtr node_; - - double prec_ = 1e-8; - RobotStatePtr start_state_; - Eigen::Isometry3d start_pose_; - std::vector> result_; + static RobotModelPtr robot_model; + static JointModelGroup* jmg; + static const LinkModel* link; + static rclcpp::Node::SharedPtr node; + + double prec = 1e-8; + RobotStatePtr start_state; + Eigen::Isometry3d start_pose; + std::vector> result; }; // TODO - The tests below fail since no kinematic plugins are found. Move the tests to IK plugin package. @@ -264,74 +264,74 @@ class PandaRobot : public testing::Test // TEST_F(PandaRobot, testVectorGlobal) // { // Eigen::Vector3d translation(0.2, 0, 0); // move by 0.2 along world's x axis -// ASSERT_DOUBLE_EQ(computeCartesianPath(result_, translation, true), 0.2); // moved full distance of 0.2 +// ASSERT_DOUBLE_EQ(computeCartesianPath(result, translation, true), 0.2); // moved full distance of 0.2 // // first pose of trajectory should be identical to start_pose -// EXPECT_EIGEN_EQ(result_.front()->getGlobalLinkTransform(link_), start_pose_); +// EXPECT_EIGEN_EQ(result.front()->getGlobalLinkTransform(link), start_pose); // // last pose of trajectory should have same orientation, and offset of 0.2 along world's x-axis -// EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_).linear(), start_pose_.linear(), prec_); -// EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_).translation(), -// start_pose_.translation() + translation, prec_); +// EXPECT_EIGEN_NEAR(result.back()->getGlobalLinkTransform(link).linear(), start_pose.linear(), prec); +// EXPECT_EIGEN_NEAR(result.back()->getGlobalLinkTransform(link).translation(), +// start_pose.translation() + translation, prec); // } // TEST_F(PandaRobot, testVectorLocal) // { // Eigen::Vector3d translation(0.2, 0, 0); // move by 0.2 along link's x axis -// ASSERT_DOUBLE_EQ(computeCartesianPath(result_, translation, false), 0.2); // moved full distance of 0.2 +// ASSERT_DOUBLE_EQ(computeCartesianPath(result, translation, false), 0.2); // moved full distance of 0.2 // // first pose of trajectory should be identical to start_pose -// EXPECT_EIGEN_EQ(result_.front()->getGlobalLinkTransform(link_), start_pose_); +// EXPECT_EIGEN_EQ(result.front()->getGlobalLinkTransform(link), start_pose); // // last pose of trajectory should have same orientation, and offset of 0.2 along link's x-axis -// EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_).linear(), start_pose_.linear(), prec_); -// EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_).translation(), start_pose_ * translation, prec_); +// EXPECT_EIGEN_NEAR(result.back()->getGlobalLinkTransform(link).linear(), start_pose.linear(), prec); +// EXPECT_EIGEN_NEAR(result.back()->getGlobalLinkTransform(link).translation(), start_pose * translation, prec); // } // TEST_F(PandaRobot, testTranslationGlobal) // { -// Eigen::Isometry3d goal = start_pose_; +// Eigen::Isometry3d goal = start_pose; // goal.translation().x() += 0.2; // move by 0.2 along world's x-axis -// ASSERT_DOUBLE_EQ(computeCartesianPath(result_, goal, true), 1.0); // 100% of distance generated +// ASSERT_DOUBLE_EQ(computeCartesianPath(result, goal, true), 1.0); // 100% of distance generated // // first pose of trajectory should be identical to start_pose -// EXPECT_EIGEN_EQ(result_.front()->getGlobalLinkTransform(link_), start_pose_); +// EXPECT_EIGEN_EQ(result.front()->getGlobalLinkTransform(link), start_pose); // // last pose of trajectory should have same orientation, but offset of 0.2 along world's x-axis -// EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_).linear(), start_pose_.linear(), prec_); -// EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_).translation(), goal.translation(), prec_); +// EXPECT_EIGEN_NEAR(result.back()->getGlobalLinkTransform(link).linear(), start_pose.linear(), prec); +// EXPECT_EIGEN_NEAR(result.back()->getGlobalLinkTransform(link).translation(), goal.translation(), prec); // } // TEST_F(PandaRobot, testTranslationLocal) // { // Eigen::Isometry3d offset(Eigen::Translation3d(0.2, 0, 0)); // move along link's x-axis -// ASSERT_DOUBLE_EQ(computeCartesianPath(result_, offset, false), 1.0); // 100% of distance generated +// ASSERT_DOUBLE_EQ(computeCartesianPath(result, offset, false), 1.0); // 100% of distance generated // // first pose of trajectory should be identical to start_pose -// EXPECT_EIGEN_EQ(result_.front()->getGlobalLinkTransform(link_), start_pose_); +// EXPECT_EIGEN_EQ(result.front()->getGlobalLinkTransform(link), start_pose); // // last pose of trajectory should have same orientation, but offset of 0.2 along link's x-axis -// EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_).linear(), start_pose_.linear(), prec_); -// EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_).translation(), start_pose_ * offset.translation(), -// prec_); +// EXPECT_EIGEN_NEAR(result.back()->getGlobalLinkTransform(link).linear(), start_pose.linear(), prec); +// EXPECT_EIGEN_NEAR(result.back()->getGlobalLinkTransform(link).translation(), start_pose * offset.translation(), +// prec); // } // TEST_F(PandaRobot, testRotationLocal) // { // // 45° rotation about links's x-axis // Eigen::Isometry3d rot(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitX())); -// Eigen::Isometry3d goal = start_pose_ * rot; +// Eigen::Isometry3d goal = start_pose * rot; -// ASSERT_DOUBLE_EQ(computeCartesianPath(result_, rot, false), 1.0); // 100% of distance generated +// ASSERT_DOUBLE_EQ(computeCartesianPath(result, rot, false), 1.0); // 100% of distance generated // // first pose of trajectory should be identical to start_pose -// EXPECT_EIGEN_EQ(result_.front()->getGlobalLinkTransform(link_), start_pose_); +// EXPECT_EIGEN_EQ(result.front()->getGlobalLinkTransform(link), start_pose); // // last pose of trajectory should have same position, -// EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_).translation(), start_pose_.translation(), prec_); -// EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_), goal, prec_); +// EXPECT_EIGEN_NEAR(result.back()->getGlobalLinkTransform(link).translation(), start_pose.translation(), prec); +// EXPECT_EIGEN_NEAR(result.back()->getGlobalLinkTransform(link), goal, prec); // } // TEST_F(PandaRobot, testRotationGlobal) // { // // 45° rotation about links's x-axis // Eigen::Isometry3d rot(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitX())); -// Eigen::Isometry3d goal = start_pose_ * rot; +// Eigen::Isometry3d goal = start_pose * rot; -// ASSERT_DOUBLE_EQ(computeCartesianPath(result_, goal, true), 1.0); // 100% of distance generated +// ASSERT_DOUBLE_EQ(computeCartesianPath(result, goal, true), 1.0); // 100% of distance generated // // first pose of trajectory should be identical to start_pose -// EXPECT_EIGEN_NEAR(result_.front()->getGlobalLinkTransform(link_), start_pose_, prec_); +// EXPECT_EIGEN_NEAR(result.front()->getGlobalLinkTransform(link), start_pose, prec); // // last pose of trajectory should have same position, -// EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_).translation(), start_pose_.translation(), prec_); -// EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_), goal, prec_); +// EXPECT_EIGEN_NEAR(result.back()->getGlobalLinkTransform(link).translation(), start_pose.translation(), prec); +// EXPECT_EIGEN_NEAR(result.back()->getGlobalLinkTransform(link), goal, prec); // } // TEST_F(PandaRobot, testRotationOffset) // { @@ -339,18 +339,18 @@ class PandaRobot : public testing::Test // Eigen::Isometry3d offset = Eigen::Translation3d(0, 0, 0.2) * Eigen::AngleAxisd(-M_PI / 4, Eigen::Vector3d::UnitZ()); // // 45° rotation about center's x-axis // Eigen::Isometry3d rot(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitX())); -// Eigen::Isometry3d goal = start_pose_ * offset * rot; +// Eigen::Isometry3d goal = start_pose * offset * rot; -// ASSERT_DOUBLE_EQ(computeCartesianPath(result_, goal, true, offset), 1.0); // 100% of distance generated +// ASSERT_DOUBLE_EQ(computeCartesianPath(result, goal, true, offset), 1.0); // 100% of distance generated // // first pose of trajectory should be identical to start_pose -// EXPECT_EIGEN_NEAR(result_.front()->getGlobalLinkTransform(link_), start_pose_, prec_); +// EXPECT_EIGEN_NEAR(result.front()->getGlobalLinkTransform(link), start_pose, prec); // // All waypoints of trajectory should have same position in virtual frame -// for (const auto& waypoint : result_) -// EXPECT_EIGEN_NEAR((waypoint->getGlobalLinkTransform(link_) * offset).translation(), -// (start_pose_ * offset).translation(), prec_); +// for (const auto& waypoint : result) +// EXPECT_EIGEN_NEAR((waypoint->getGlobalLinkTransform(link) * offset).translation(), +// (start_pose * offset).translation(), prec); // // goal should be reached by virtual frame -// EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_) * offset, goal, prec_); +// EXPECT_EIGEN_NEAR(result.back()->getGlobalLinkTransform(link) * offset, goal, prec); // } int main(int argc, char** argv) diff --git a/moveit_core/robot_state/test/test_kinematic_complex.cpp b/moveit_core/robot_state/test/test_kinematic_complex.cpp index 2c5e64dd91..973c5560a5 100644 --- a/moveit_core/robot_state/test/test_kinematic_complex.cpp +++ b/moveit_core/robot_state/test/test_kinematic_complex.cpp @@ -311,7 +311,7 @@ TEST_F(LoadPlanningModelsPr2, ObjectPoseAndSubframes) // Add another object C that is defined in a frame that is not the link. // The object will be transformed into the link's frame, which - // uses an otherwise inactive section of _msgToAttachedBody. + // uses an otherwise inactive section of msgToAttachedBody. Eigen::Isometry3d pose_c = Eigen::Isometry3d(Eigen::Translation3d(0.1, 0.2, 0.3)) * Eigen::AngleAxisd(0.1 * M_TAU, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(0.2 * M_TAU, Eigen::Vector3d::UnitY()) * diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index 26e7f9da4f..35533d173c 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -396,7 +396,7 @@ std::ostream& operator<<(std::ostream& out, const RobotTrajectory& trajectory); /// active joint distances between the two states (L1 norm). /// \param[in] trajectory Given robot trajectory /// \return Length of the robot trajectory [rad] -[[nodiscard]] double path_length(const RobotTrajectory& trajectory); +[[nodiscard]] double pathLength(const RobotTrajectory& trajectory); /// \brief Calculate the smoothness of a given trajectory /// \param[in] trajectory Given robot trajectory @@ -408,6 +408,6 @@ std::ostream& operator<<(std::ostream& out, const RobotTrajectory& trajectory); /// \param[in] trajectory Given robot trajectory /// \return Waypoint density of the given trajectory /// or nullopt if it is not possible to calculate the density -[[nodiscard]] std::optional waypoint_density(const RobotTrajectory& trajectory); +[[nodiscard]] std::optional waypointDensity(const RobotTrajectory& trajectory); } // namespace robot_trajectory diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index 8709ed35e2..2fe07f1e30 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -631,7 +631,7 @@ std::ostream& operator<<(std::ostream& out, const RobotTrajectory& trajectory) return out; } -double path_length(const RobotTrajectory& trajectory) +double pathLength(const RobotTrajectory& trajectory) { auto trajectory_length = 0.0; for (std::size_t index = 1; index < trajectory.getWayPointCount(); ++index) @@ -681,13 +681,13 @@ std::optional smoothness(const RobotTrajectory& trajectory) return std::nullopt; } -std::optional waypoint_density(const RobotTrajectory& trajectory) +std::optional waypointDensity(const RobotTrajectory& trajectory) { // Only calculate density if more than one waypoint exists if (trajectory.getWayPointCount() > 1) { // Calculate path length - const auto length = path_length(trajectory); + const auto length = pathLength(trajectory); if (length > 0.0) { auto density = static_cast(trajectory.getWayPointCount()) / length; diff --git a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp index a2de86e8df..1d0a246eac 100644 --- a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp @@ -528,7 +528,7 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryLength) { robot_trajectory::RobotTrajectoryPtr trajectory; initTestTrajectory(trajectory); - EXPECT_GT(robot_trajectory::path_length(*trajectory), 0.0); + EXPECT_GT(robot_trajectory::pathLength(*trajectory), 0.0); } TEST_F(RobotTrajectoryTestFixture, RobotTrajectorySmoothness) @@ -550,13 +550,13 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryDensity) robot_trajectory::RobotTrajectoryPtr trajectory; initTestTrajectory(trajectory); - const auto density = robot_trajectory::waypoint_density(*trajectory); + const auto density = robot_trajectory::waypointDensity(*trajectory); ASSERT_TRUE(density.has_value()); EXPECT_GT(density.value(), 0.0); // Check for empty trajectory trajectory->clear(); - EXPECT_FALSE(robot_trajectory::waypoint_density(*trajectory).has_value()); + EXPECT_FALSE(robot_trajectory::waypointDensity(*trajectory).has_value()); } TEST_F(OneRobot, Unwind) 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 70a6272df1..9a0319024b 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -49,8 +49,7 @@ namespace trajectory_processing { namespace { -static const rclcpp::Logger LOGGER = - rclcpp::get_logger("moveit_trajectory_processing.time_optimal_trajectory_generation"); +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_trajectory_processing.time_optimal_trajectory_generation"); constexpr double DEFAULT_TIMESTEP = 1e-3; constexpr double EPS = 1e-6; constexpr double DEFAULT_SCALING_FACTOR = 1.0; 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 71a6b9c147..b83f7ffadb 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 @@ -50,7 +50,7 @@ namespace // so add them here. // TODO(andyz): Function won't be needed once this issue has been addressed: // https://github.com/ros/urdfdom/issues/177 -void set_acceleration_limits(const moveit::core::RobotModelPtr& robot_model) +void setAccelerationLimits(const moveit::core::RobotModelPtr& robot_model) { const std::vector joint_models = robot_model->getActiveJointModels(); for (auto& joint_model : joint_models) @@ -198,7 +198,7 @@ TEST(time_optimal_trajectory_generation, testCustomLimits) auto robot_model = moveit::core::loadTestingRobotModel(robot_name); ASSERT_TRUE(robot_model) << "Failed to load robot model" << robot_name; - set_acceleration_limits(robot_model); + setAccelerationLimits(robot_model); auto group = robot_model->getJointModelGroup(group_name); ASSERT_TRUE(group) << "Failed to load joint model group " << group_name; moveit::core::RobotState waypoint_state(robot_model); @@ -310,7 +310,7 @@ TEST(time_optimal_trajectory_generation, testLastWaypoint) auto robot_model = moveit::core::loadTestingRobotModel(robot_name); ASSERT_TRUE(robot_model) << "Failed to load robot model" << robot_name; - set_acceleration_limits(robot_model); + setAccelerationLimits(robot_model); auto group = robot_model->getJointModelGroup(group_name); ASSERT_TRUE(group) << "Failed to load joint model group " << group_name; moveit::core::RobotState waypoint_state(robot_model); @@ -349,7 +349,7 @@ TEST(time_optimal_trajectory_generation, testPluginAPI) auto robot_model = moveit::core::loadTestingRobotModel(robot_name); ASSERT_TRUE(robot_model) << "Failed to load robot model" << robot_name; - set_acceleration_limits(robot_model); + setAccelerationLimits(robot_model); auto group = robot_model->getJointModelGroup(group_name); ASSERT_TRUE(group) << "Failed to load joint model group " << group_name; moveit::core::RobotState waypoint_state(robot_model); @@ -472,7 +472,7 @@ TEST(time_optimal_trajectory_generation, testFixedNumWaypoints) auto robot_model = moveit::core::loadTestingRobotModel(robot_name); ASSERT_TRUE(robot_model) << "Failed to load robot model" << robot_name; - set_acceleration_limits(robot_model); + setAccelerationLimits(robot_model); auto group = robot_model->getJointModelGroup(group_name); ASSERT_TRUE(group) << "Failed to load joint model group " << group_name; moveit::core::RobotState waypoint_state(robot_model); diff --git a/moveit_core/utils/include/moveit/utils/moveit_error_code.h b/moveit_core/utils/include/moveit/utils/moveit_error_code.h index b948cf3201..5f920744d0 100644 --- a/moveit_core/utils/include/moveit/utils/moveit_error_code.h +++ b/moveit_core/utils/include/moveit/utils/moveit_error_code.h @@ -74,7 +74,7 @@ class MoveItErrorCode : public moveit_msgs::msg::MoveItErrorCodes @param error_code Error code to be translated to a string @return Error code string */ -inline std::string error_code_to_string(MoveItErrorCode error_code) +inline std::string errorCodeToString(MoveItErrorCode error_code) { switch (error_code.val) { 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 e59918f137..802fb89759 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 @@ -79,7 +79,7 @@ srdf::ModelSharedPtr loadSRDFModel(const std::string& robot_name); * \param[in] plugin name of the plugin ("KDL", or full name) * \param[in] timeout default solver timeout */ -void loadIKPluginForGroup(rclcpp::Node::SharedPtr node, JointModelGroup* jmg, const std::string& base_link, +void loadIKPluginForGroup(const rclcpp::Node::SharedPtr& node, JointModelGroup* jmg, const std::string& base_link, const std::string& tip_link, std::string plugin = "KDL", double timeout = 0.1); /** \brief Easily build different robot models for testing. diff --git a/moveit_core/utils/src/robot_model_test_utils.cpp b/moveit_core/utils/src/robot_model_test_utils.cpp index f4713efdf8..505efe2b9b 100644 --- a/moveit_core/utils/src/robot_model_test_utils.cpp +++ b/moveit_core/utils/src/robot_model_test_utils.cpp @@ -104,7 +104,7 @@ srdf::ModelSharedPtr loadSRDFModel(const std::string& robot_name) return srdf_model; } -void loadIKPluginForGroup(rclcpp::Node::SharedPtr node, JointModelGroup* jmg, const std::string& base_link, +void loadIKPluginForGroup(const rclcpp::Node::SharedPtr& node, JointModelGroup* jmg, const std::string& base_link, const std::string& tip_link, std::string plugin, double timeout) { using LoaderType = pluginlib::ClassLoader; diff --git a/moveit_planners/ompl/ompl_interface/scripts/generate_state_database.cpp b/moveit_planners/ompl/ompl_interface/scripts/generate_state_database.cpp index 5d7bb2bb95..ca1b90114c 100644 --- a/moveit_planners/ompl/ompl_interface/scripts/generate_state_database.cpp +++ b/moveit_planners/ompl/ompl_interface/scripts/generate_state_database.cpp @@ -54,8 +54,8 @@ static const std::string ROBOT_DESCRIPTION = "robot_description"; static const std::string CONSTRAINT_PARAMETER = "constraints"; -static bool get_uint_parameter_or(const rclcpp::Node::SharedPtr& node, const std::string& param_name, - size_t&& result_value, const size_t default_value) +static bool getUintParameterOr(const rclcpp::Node::SharedPtr& node, const std::string& param_name, + size_t&& result_value, const size_t default_value) { int param_value; if (node->get_parameter(param_name, param_value)) @@ -83,17 +83,17 @@ struct GenerateStateDatabaseParameters node->get_parameter_or("use_current_scene", use_current_scene, false); // number of states in joint space approximation - get_uint_parameter_or(node, "state_cnt", construction_opts.samples, 10000); + getUintParameterOr(node, "state_cnt", construction_opts.samples, 10000); // generate edges together with states? - get_uint_parameter_or(node, "edges_per_sample", construction_opts.edges_per_sample, 0); + getUintParameterOr(node, "edges_per_sample", construction_opts.edges_per_sample, 0); node->get_parameter_or("max_edge_length", construction_opts.max_edge_length, 0.2); // verify constraint validity on edges node->get_parameter_or("explicit_motions", construction_opts.explicit_motions, true); node->get_parameter_or("explicit_points_resolution", construction_opts.explicit_points_resolution, 0.05); - get_uint_parameter_or(node, "max_explicit_points", construction_opts.max_explicit_points, 200); + getUintParameterOr(node, "max_explicit_points", construction_opts.max_explicit_points, 200); // local planning in JointModel state space node->get_parameter_or("state_space_parameterization", construction_opts.state_space_parameterization, diff --git a/moveit_planners/pilz_industrial_motion_planner/include/joint_limits_copy/joint_limits_rosparam.hpp b/moveit_planners/pilz_industrial_motion_planner/include/joint_limits_copy/joint_limits_rosparam.hpp index d41ac0aca6..bb49c314ef 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/joint_limits_copy/joint_limits_rosparam.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/joint_limits_copy/joint_limits_rosparam.hpp @@ -30,7 +30,7 @@ namespace { /** declare a parameter if not already declared. */ template -void declare_parameter(const rclcpp::Node::SharedPtr& node, const std::string& name, T default_value) +void declareParameter(const rclcpp::Node::SharedPtr& node, const std::string& name, T default_value) { if (not node->has_parameter(name)) { @@ -48,24 +48,24 @@ inline bool declare_parameters(const std::string& joint_name, const rclcpp::Node try { - declare_parameter(node, param_base_name + ".has_position_limits", false); - declare_parameter(node, param_base_name + ".min_position", std::numeric_limits::quiet_NaN()); - declare_parameter(node, param_base_name + ".max_position", std::numeric_limits::quiet_NaN()); - declare_parameter(node, param_base_name + ".has_velocity_limits", false); - declare_parameter(node, param_base_name + ".min_velocity", std::numeric_limits::quiet_NaN()); - declare_parameter(node, param_base_name + ".max_velocity", std::numeric_limits::quiet_NaN()); - declare_parameter(node, param_base_name + ".has_acceleration_limits", false); - declare_parameter(node, param_base_name + ".max_acceleration", std::numeric_limits::quiet_NaN()); - declare_parameter(node, param_base_name + ".has_jerk_limits", false); - declare_parameter(node, param_base_name + ".max_jerk", std::numeric_limits::quiet_NaN()); - declare_parameter(node, param_base_name + ".has_effort_limits", false); - declare_parameter(node, param_base_name + ".max_effort", std::numeric_limits::quiet_NaN()); - declare_parameter(node, param_base_name + ".angle_wraparound", false); - declare_parameter(node, param_base_name + ".has_soft_limits", false); - declare_parameter(node, param_base_name + ".k_position", std::numeric_limits::quiet_NaN()); - declare_parameter(node, param_base_name + ".k_velocity", std::numeric_limits::quiet_NaN()); - declare_parameter(node, param_base_name + ".soft_lower_limit", std::numeric_limits::quiet_NaN()); - declare_parameter(node, param_base_name + ".soft_upper_limit", std::numeric_limits::quiet_NaN()); + declareParameter(node, param_base_name + ".has_position_limits", false); + declareParameter(node, param_base_name + ".min_position", std::numeric_limits::quiet_NaN()); + declareParameter(node, param_base_name + ".max_position", std::numeric_limits::quiet_NaN()); + declareParameter(node, param_base_name + ".has_velocity_limits", false); + declareParameter(node, param_base_name + ".min_velocity", std::numeric_limits::quiet_NaN()); + declareParameter(node, param_base_name + ".max_velocity", std::numeric_limits::quiet_NaN()); + declareParameter(node, param_base_name + ".has_acceleration_limits", false); + declareParameter(node, param_base_name + ".max_acceleration", std::numeric_limits::quiet_NaN()); + declareParameter(node, param_base_name + ".has_jerk_limits", false); + declareParameter(node, param_base_name + ".max_jerk", std::numeric_limits::quiet_NaN()); + declareParameter(node, param_base_name + ".has_effort_limits", false); + declareParameter(node, param_base_name + ".max_effort", std::numeric_limits::quiet_NaN()); + declareParameter(node, param_base_name + ".angle_wraparound", false); + declareParameter(node, param_base_name + ".has_soft_limits", false); + declareParameter(node, param_base_name + ".k_position", std::numeric_limits::quiet_NaN()); + declareParameter(node, param_base_name + ".k_velocity", std::numeric_limits::quiet_NaN()); + declareParameter(node, param_base_name + ".soft_lower_limit", std::numeric_limits::quiet_NaN()); + declareParameter(node, param_base_name + ".soft_upper_limit", std::numeric_limits::quiet_NaN()); } catch (const std::exception& ex) { @@ -109,8 +109,8 @@ inline bool declare_parameters(const std::string& joint_name, const rclcpp::Node * \return True if a limits specification is found (ie. the \p joint_limits/joint_name parameter exists in \p node), * false otherwise. */ -inline bool get_joint_limits(const std::string& joint_name, const rclcpp::Node::SharedPtr& node, - const std::string& param_ns, JointLimits& limits) +inline bool getJointLimits(const std::string& joint_name, const rclcpp::Node::SharedPtr& node, + const std::string& param_ns, JointLimits& limits) { const std::string param_base_name = (param_ns.empty() ? "" : param_ns + ".") + "joint_limits." + joint_name; try @@ -254,8 +254,8 @@ inline bool get_joint_limits(const std::string& joint_name, const rclcpp::Node:: * \p k_velocity, \p soft_lower_limit and \p soft_upper_limit exist in \p joint_limits/joint_name namespace), false * otherwise. */ -inline bool get_joint_limits(const std::string& joint_name, const rclcpp::Node::SharedPtr& node, - const std::string& param_ns, SoftJointLimits& soft_limits) +inline bool getJointLimits(const std::string& joint_name, const rclcpp::Node::SharedPtr& node, + const std::string& param_ns, SoftJointLimits& soft_limits) { const std::string param_base_name = (param_ns.empty() ? "" : param_ns + ".") + "joint_limits." + joint_name; try diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h index 97b3be3936..0d43d11487 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h @@ -52,13 +52,13 @@ inline bool declareParameters(const std::string& joint_name, const std::string& return joint_limits::declare_parameters(joint_name, node, param_ns); } /** - * @see joint_limits::get_joint_limits(...) + * @see joint_limits::getJointLimits(...) */ inline bool getJointLimits(const std::string& joint_name, const std::string& param_ns, const rclcpp::Node::SharedPtr& node, joint_limits_interface::JointLimits& limits) { // Load the existing limits - if (!joint_limits::get_joint_limits(joint_name, node, param_ns, limits)) + if (!joint_limits::getJointLimits(joint_name, node, param_ns, limits)) { return false; // LCOV_EXCL_LINE // The case where getJointLimits returns // false is covered above. diff --git a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp index bdbf31f662..d8fe742772 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp @@ -51,8 +51,11 @@ namespace pilz_industrial_motion_planner { -static const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning"; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.command_list_manager"); +namespace +{ +const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning"; +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.command_list_manager"); +} // namespace CommandListManager::CommandListManager(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& model) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp index a07f51b196..feb0a49525 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp @@ -41,8 +41,7 @@ #include namespace { -static const rclcpp::Logger LOGGER = - rclcpp::get_logger("moveit.pilz_industrial_motion_planner.joint_limits_aggregator"); +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.joint_limits_aggregator"); } pilz_industrial_motion_planner::JointLimitsContainer diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp index 6966f47e7c..79a9584387 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp @@ -41,7 +41,10 @@ namespace pilz_industrial_motion_planner { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.joint_limits_container"); +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.joint_limits_container"); +} bool JointLimitsContainer::addLimit(const std::string& joint_name, pilz_industrial_motion_planner::JointLimit joint_limit) { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp index e607393aa5..4784ef145a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp @@ -37,7 +37,10 @@ namespace pilz_industrial_motion_planner { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("pilz_industrial_motion_planner.limits_container"); +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("pilz_industrial_motion_planner.limits_container"); +} LimitsContainer::LimitsContainer() : has_joint_limits_(false), has_cartesian_limits_(false) { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp index e1465fbfb5..691aa22dbc 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp @@ -53,8 +53,10 @@ namespace pilz_industrial_motion_planner { -static const rclcpp::Logger LOGGER = - rclcpp::get_logger("moveit.pilz_industrial_motion_planner.move_group_sequence_action"); +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.move_group_sequence_action"); +} MoveGroupSequenceAction::MoveGroupSequenceAction() : MoveGroupCapability("SequenceAction") diff --git a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp index d2845ac5af..44d7405511 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp @@ -45,8 +45,10 @@ #include namespace pilz_industrial_motion_planner { -static const rclcpp::Logger LOGGER = - rclcpp::get_logger("moveit.pilz_industrial_motion_planner.move_group_sequence_service"); +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.move_group_sequence_service"); +} MoveGroupSequenceService::MoveGroupSequenceService() : MoveGroupCapability("SequenceService") { } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp index 9ddfc1baf9..cdadff31f2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp @@ -51,8 +51,11 @@ namespace pilz_industrial_motion_planner { -static const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning"; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner"); +namespace +{ +const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning"; +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner"); +} // namespace bool CommandPlanner::initialize(const moveit::core::RobotModelConstPtr& model, const rclcpp::Node::SharedPtr& node, const std::string& ns) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp index b149155780..dfe23638c4 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp @@ -44,8 +44,7 @@ namespace { -static const rclcpp::Logger LOGGER = - rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_loader_circ"); +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_loader_circ"); } pilz_industrial_motion_planner::PlanningContextLoaderCIRC::PlanningContextLoaderCIRC() diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp index 81b90ff1d9..08c25e1a28 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp @@ -41,8 +41,7 @@ namespace { -static const rclcpp::Logger LOGGER = - rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_loader_lin"); +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_loader_lin"); } pilz_industrial_motion_planner::PlanningContextLoaderLIN::PlanningContextLoaderLIN() diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp index 4cc9b7d28d..4eda1c472a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp @@ -40,8 +40,7 @@ namespace { -static const rclcpp::Logger LOGGER = - rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_loader_ptp"); +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_loader_ptp"); } pilz_industrial_motion_planner::PlanningContextLoaderPTP::PlanningContextLoaderPTP() diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp index 920c43530a..b4c0f0145a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp @@ -42,7 +42,7 @@ namespace { -static const rclcpp::Logger LOGGER = +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_blender_transition_window"); } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index 97f2d6380e..9f1e5f795c 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -42,7 +42,7 @@ namespace { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_functions"); +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_functions"); } bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::PlanningSceneConstPtr& scene, diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp index 75bb14d7ac..0b67c62013 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -47,7 +47,10 @@ namespace pilz_industrial_motion_planner { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator"); +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator"); +} sensor_msgs::msg::JointState TrajectoryGenerator::filterGroupValues(const sensor_msgs::msg::JointState& robot_state, const std::string& group) const diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp index 2748e11fe6..f8afbbdfff 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp @@ -51,8 +51,10 @@ namespace pilz_industrial_motion_planner { -static const rclcpp::Logger LOGGER = - rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator_circ"); +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator_circ"); +} TrajectoryGeneratorCIRC::TrajectoryGeneratorCIRC(const moveit::core::RobotModelConstPtr& robot_model, const LimitsContainer& planner_limits, const std::string& /*group_name*/) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp index 50e25a8f7e..8147f64af6 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -52,8 +52,10 @@ namespace pilz_industrial_motion_planner { -static const rclcpp::Logger LOGGER = - rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator_lin"); +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator_lin"); +} TrajectoryGeneratorLIN::TrajectoryGeneratorLIN(const moveit::core::RobotModelConstPtr& robot_model, const LimitsContainer& planner_limits, const std::string& /*group_name*/) : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp index 95623762d1..b0344350ed 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp @@ -46,8 +46,10 @@ namespace pilz_industrial_motion_planner { -static const rclcpp::Logger LOGGER = - rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator_ptp"); +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator_ptp"); +} TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const moveit::core::RobotModelConstPtr& robot_model, const LimitsContainer& planner_limits, const std::string& group_name) : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) diff --git a/moveit_planners/stomp/include/stomp_moveit/conversion_functions.hpp b/moveit_planners/stomp/include/stomp_moveit/conversion_functions.hpp index f9537a7760..20c5d83773 100644 --- a/moveit_planners/stomp/include/stomp_moveit/conversion_functions.hpp +++ b/moveit_planners/stomp/include/stomp_moveit/conversion_functions.hpp @@ -54,7 +54,7 @@ using Joints = std::vector; * * @return The vector containing the joint values */ -std::vector get_positions(const moveit::core::RobotState& state, const Joints& joints) +std::vector getPositions(const moveit::core::RobotState& state, const Joints& joints) { std::vector positions; for (const auto& joint : joints) @@ -74,7 +74,7 @@ std::vector get_positions(const moveit::core::RobotState& state, const J * @param joints The joints that should be considered * @param state The robot state to update with the new joint values */ -void set_joint_positions(const Eigen::VectorXd& values, const Joints& joints, moveit::core::RobotState& state) +void setJointPositions(const Eigen::VectorXd& values, const Joints& joints, moveit::core::RobotState& state) { for (size_t joint_index = 0; joint_index < joints.size(); ++joint_index) { @@ -89,8 +89,8 @@ void set_joint_positions(const Eigen::VectorXd& values, const Joints& joints, mo * @param reference_state A robot state providing default joint values and robot model * @param trajectory The robot trajectory containing waypoints with updated values */ -void fill_robot_trajectory(const Eigen::MatrixXd& trajectory_values, const moveit::core::RobotState& reference_state, - robot_trajectory::RobotTrajectory& trajectory) +void fillRobotTrajectory(const Eigen::MatrixXd& trajectory_values, const moveit::core::RobotState& reference_state, + robot_trajectory::RobotTrajectory& trajectory) { trajectory.clear(); const auto& active_joints = trajectory.getGroup() ? trajectory.getGroup()->getActiveJointModels() : @@ -100,7 +100,7 @@ void fill_robot_trajectory(const Eigen::MatrixXd& trajectory_values, const movei for (int timestep = 0; timestep < trajectory_values.cols(); ++timestep) { const auto waypoint = std::make_shared(reference_state); - set_joint_positions(trajectory_values.col(timestep), active_joints, *waypoint); + setJointPositions(trajectory_values.col(timestep), active_joints, *waypoint); trajectory.addSuffixWayPoint(waypoint, 0.1 /* placeholder dt */); } @@ -115,12 +115,12 @@ void fill_robot_trajectory(const Eigen::MatrixXd& trajectory_values, const movei * * @return The created RobotTrajectory containing updated waypoints */ -robot_trajectory::RobotTrajectory matrix_to_robot_trajectory(const Eigen::MatrixXd& trajectory_values, - const moveit::core::RobotState& reference_state, - const moveit::core::JointModelGroup* group = nullptr) +robot_trajectory::RobotTrajectory matrixToRobotTrajectory(const Eigen::MatrixXd& trajectory_values, + const moveit::core::RobotState& reference_state, + const moveit::core::JointModelGroup* group = nullptr) { robot_trajectory::RobotTrajectory trajectory(reference_state.getRobotModel(), group); - fill_robot_trajectory(trajectory_values, reference_state, trajectory); + fillRobotTrajectory(trajectory_values, reference_state, trajectory); return trajectory; } @@ -131,7 +131,7 @@ robot_trajectory::RobotTrajectory matrix_to_robot_trajectory(const Eigen::Matrix * * @return The matrix representing a sequence of waypoint positions */ -Eigen::MatrixXd robot_trajectory_to_matrix(const robot_trajectory::RobotTrajectory& trajectory) +Eigen::MatrixXd robotTrajectoryToMatrix(const robot_trajectory::RobotTrajectory& trajectory) { const auto& active_joints = trajectory.getGroup() ? trajectory.getGroup()->getActiveJointModels() : trajectory.getRobotModel()->getActiveJointModels(); diff --git a/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp b/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp index e41a7cac5b..4d1dd2993a 100644 --- a/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp +++ b/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp @@ -72,7 +72,7 @@ constexpr double CONSTRAINT_CHECK_DISTANCE = 0.05; * * @return Cost function that computes smooth costs for binary validity conditions */ -CostFn get_cost_function_from_state_validator(const StateValidatorFn& state_validator_fn, double interpolation_step_size) +CostFn getCostFunctionFromStateValidator(const StateValidatorFn& state_validator_fn, double interpolation_step_size) { CostFn cost_fn = [=](const Eigen::MatrixXd& values, Eigen::VectorXd& costs, bool& validity) { costs.setZero(values.cols()); @@ -157,7 +157,7 @@ CostFn get_cost_function_from_state_validator(const StateValidatorFn& state_vali /** * Creates a cost function for binary collisions of group states in the planning scene. * This function uses a StateValidatorFn for computing smooth penalty costs from binary - * collision checks using get_cost_function_from_state_validator(). + * collision checks using getCostFunctionFromStateValidator(). * * @param planning_scene The planning scene instance to use for collision checking * @param group The group to use for computing link transforms from joint positions @@ -165,8 +165,8 @@ CostFn get_cost_function_from_state_validator(const StateValidatorFn& state_vali * * @return Cost function that computes smooth costs for colliding path segments */ -CostFn get_collision_cost_function(const std::shared_ptr& planning_scene, - const moveit::core::JointModelGroup* group, double collision_penalty) +CostFn getCollisionCostFunction(const std::shared_ptr& planning_scene, + const moveit::core::JointModelGroup* group, double collision_penalty) { const auto& joints = group ? group->getActiveJointModels() : planning_scene->getRobotModel()->getActiveJointModels(); const auto& group_name = group ? group->getName() : ""; @@ -175,19 +175,19 @@ CostFn get_collision_cost_function(const std::shared_ptrgetCurrentState()); // Update robot state values - set_joint_positions(positions, joints, state); + setJointPositions(positions, joints, state); state.update(); return planning_scene->isStateColliding(state, group_name) ? collision_penalty : 0.0; }; - return get_cost_function_from_state_validator(collision_validator_fn, COL_CHECK_DISTANCE); + return getCostFunctionFromStateValidator(collision_validator_fn, COL_CHECK_DISTANCE); } /** * Creates a cost function for binary constraint checks applied to group states. * This function uses a StateValidatorFn for computing smooth penalty costs from binary - * constraint checks using get_cost_function_from_state_validator(). + * constraint checks using getCostFunctionFromStateValidator(). * * @param planning_scene The planning scene instance to use for computing transforms * @param group The group to use for computing link transforms from joint positions @@ -196,9 +196,9 @@ CostFn get_collision_cost_function(const std::shared_ptr& planning_scene, - const moveit::core::JointModelGroup* group, - const moveit_msgs::msg::Constraints& constraints_msg, double cost_scale) +CostFn getConstraintsCostFunction(const std::shared_ptr& planning_scene, + const moveit::core::JointModelGroup* group, + const moveit_msgs::msg::Constraints& constraints_msg, double cost_scale) { const auto& joints = group ? group->getActiveJointModels() : planning_scene->getRobotModel()->getActiveJointModels(); @@ -209,13 +209,13 @@ CostFn get_constraints_cost_function(const std::shared_ptrgetCurrentState()); // Update robot state values - set_joint_positions(positions, joints, state); + setJointPositions(positions, joints, state); state.update(); return constraints.decide(state).distance * cost_scale; }; - return get_cost_function_from_state_validator(constraints_validator_fn, CONSTRAINT_CHECK_DISTANCE); + return getCostFunctionFromStateValidator(constraints_validator_fn, CONSTRAINT_CHECK_DISTANCE); } /** diff --git a/moveit_planners/stomp/include/stomp_moveit/filter_functions.hpp b/moveit_planners/stomp/include/stomp_moveit/filter_functions.hpp index 8d96090a69..f1ec47b3bb 100644 --- a/moveit_planners/stomp/include/stomp_moveit/filter_functions.hpp +++ b/moveit_planners/stomp/include/stomp_moveit/filter_functions.hpp @@ -60,7 +60,7 @@ static const FilterFn NO_FILTER = [](const Eigen::MatrixXd& /*values*/, Eigen::M * @param num_timesteps The number of trajectory waypoints configured for STOMP * @return The smoothing filter function to be used for the STOMP task */ -FilterFn simple_smoothing_matrix(size_t num_timesteps) +FilterFn simpleSmoothingMatrix(size_t num_timesteps) { // Generates a smoothing matrix and applies it for each joint dimension in filtered_values // The 'dt' value is a placeholder timestep duration that is used for approximating the second order derivative @@ -83,7 +83,7 @@ FilterFn simple_smoothing_matrix(size_t num_timesteps) * @param group The JointModelGroup providing the joint limits * @return The filter function for enforcing joint limits */ -FilterFn enforce_position_bounds(const moveit::core::JointModelGroup* group) +FilterFn enforcePositionBounds(const moveit::core::JointModelGroup* group) { return [=](const Eigen::MatrixXd& values, Eigen::MatrixXd& filtered_values) { filtered_values = values; diff --git a/moveit_planners/stomp/include/stomp_moveit/noise_generators.hpp b/moveit_planners/stomp/include/stomp_moveit/noise_generators.hpp index e76669f8ef..17b681c62c 100644 --- a/moveit_planners/stomp/include/stomp_moveit/noise_generators.hpp +++ b/moveit_planners/stomp/include/stomp_moveit/noise_generators.hpp @@ -55,7 +55,7 @@ namespace noise * @param num_timesteps the waypoint count of the trajectory * @param stddev the standard deviation for each variable dimension (number of joints) */ -NoiseGeneratorFn get_normal_distribution_generator(size_t num_timesteps, const std::vector& stddev) +NoiseGeneratorFn getNormalDistributionGenerator(size_t num_timesteps, const std::vector& stddev) { // Five-point stencil constants static const std::vector ACC_MATRIX_DIAGONAL_VALUES = { -1.0 / 12.0, 16.0 / 12.0, -30.0 / 12.0, 16.0 / 12.0, diff --git a/moveit_planners/stomp/include/stomp_moveit/trajectory_visualization.hpp b/moveit_planners/stomp/include/stomp_moveit/trajectory_visualization.hpp index 537898b108..518f73c855 100644 --- a/moveit_planners/stomp/include/stomp_moveit/trajectory_visualization.hpp +++ b/moveit_planners/stomp/include/stomp_moveit/trajectory_visualization.hpp @@ -117,9 +117,9 @@ createTrajectoryMarkerArray(const robot_trajectory::RobotTrajectory& robot_traje * returned. Otherwise, a function that does nothing. */ PostIterationFn -get_iteration_path_publisher(const rclcpp::Publisher::SharedPtr& marker_publisher, - const std::shared_ptr& planning_scene, - const moveit::core::JointModelGroup* group) +getIterationPathPublisher(const rclcpp::Publisher::SharedPtr& marker_publisher, + const std::shared_ptr& planning_scene, + const moveit::core::JointModelGroup* group) { assert(group != nullptr); @@ -134,7 +134,7 @@ get_iteration_path_publisher(const rclcpp::PublishergetCurrentState())]( int /*iteration_number*/, double /*cost*/, const Eigen::MatrixXd& values) { static thread_local robot_trajectory::RobotTrajectory trajectory(reference_state.getRobotModel(), group); - fill_robot_trajectory(values, reference_state, trajectory); + fillRobotTrajectory(values, reference_state, trajectory); const moveit::core::LinkModel* ee_parent_link = group->getOnlyOneEndEffectorTip(); @@ -156,10 +156,10 @@ get_iteration_path_publisher(const rclcpp::Publisher::SharedPtr& marker_publisher, - const std::shared_ptr& planning_scene, - const moveit::core::JointModelGroup* group) +DoneFn +getSuccessTrajectoryPublisher(const rclcpp::Publisher::SharedPtr& marker_publisher, + const std::shared_ptr& planning_scene, + const moveit::core::JointModelGroup* group) { assert(group != nullptr); @@ -176,7 +176,7 @@ DoneFn get_success_trajectory_publisher( static thread_local robot_trajectory::RobotTrajectory trajectory(reference_state.getRobotModel(), group); if (success) { - fill_robot_trajectory(values, reference_state, trajectory); + fillRobotTrajectory(values, reference_state, trajectory); const moveit::core::LinkModel* ee_parent_link = group->getOnlyOneEndEffectorTip(); diff --git a/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp b/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp index d37215a8b3..0b0ddb30d7 100644 --- a/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp +++ b/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp @@ -67,17 +67,17 @@ bool solveWithStomp(const std::shared_ptr& stomp, const moveit::co bool success = false; if (!input_trajectory || input_trajectory->empty()) { - success = stomp->solve(get_positions(start_state, joints), get_positions(goal_state, joints), waypoints); + success = stomp->solve(getPositions(start_state, joints), getPositions(goal_state, joints), waypoints); } else { - auto input = robot_trajectory_to_matrix(*input_trajectory); + auto input = robotTrajectoryToMatrix(*input_trajectory); success = stomp->solve(input, waypoints); } if (success) { output_trajectory = std::make_shared(start_state.getRobotModel(), group); - fill_robot_trajectory(waypoints, start_state, *output_trajectory); + fillRobotTrajectory(waypoints, start_state, *output_trajectory); } return success; @@ -154,24 +154,24 @@ stomp::TaskPtr createStompTask(const stomp::StompConfiguration& config, StompPla CostFn cost_fn; if (!constraints.empty()) { - cost_fn = costs::sum({ costs::get_collision_cost_function(planning_scene, group, 1.0 /* collision penalty */), - costs::get_constraints_cost_function(planning_scene, group, constraints.getAllConstraints(), - 1.0 /* constraint penalty */) }); + cost_fn = costs::sum({ costs::getCollisionCostFunction(planning_scene, group, 1.0 /* collision penalty */), + costs::getConstraintsCostFunction(planning_scene, group, constraints.getAllConstraints(), + 1.0 /* constraint penalty */) }); } else { - cost_fn = costs::get_collision_cost_function(planning_scene, group, 1.0 /* collision penalty */); + cost_fn = costs::getCollisionCostFunction(planning_scene, group, 1.0 /* collision penalty */); } // TODO(henningkayser): parameterize stddev const std::vector stddev(group->getActiveJointModels().size(), 0.1); - auto noise_generator_fn = noise::get_normal_distribution_generator(num_timesteps, stddev); + auto noise_generator_fn = noise::getNormalDistributionGenerator(num_timesteps, stddev); auto filter_fn = - filters::chain({ filters::simple_smoothing_matrix(num_timesteps), filters::enforce_position_bounds(group) }); + filters::chain({ filters::simpleSmoothingMatrix(num_timesteps), filters::enforcePositionBounds(group) }); auto iteration_callback_fn = - visualization::get_iteration_path_publisher(context.getPathPublisher(), planning_scene, group); + visualization::getIterationPathPublisher(context.getPathPublisher(), planning_scene, group); auto done_callback_fn = - visualization::get_success_trajectory_publisher(context.getPathPublisher(), planning_scene, group); + visualization::getSuccessTrajectoryPublisher(context.getPathPublisher(), planning_scene, group); // Initialize and return STOMP task stomp::TaskPtr task = diff --git a/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp b/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp index 474c17674f..c7b49e0751 100644 --- a/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp +++ b/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp @@ -135,14 +135,13 @@ inline double& variable(control_msgs::msg::JointTolerance& msg) return msg.acceleration; } -static const std::map VAR_NAME = { { POSITION, "position" }, - { VELOCITY, "velocity" }, - { ACCELERATION, "acceleration" } }; -static const std::map)> VAR_ACCESS = { - { POSITION, &variable }, - { VELOCITY, &variable }, - { ACCELERATION, &variable } -}; +const std::map VAR_NAME = { { POSITION, "position" }, + { VELOCITY, "velocity" }, + { ACCELERATION, "acceleration" } }; +const std::map)> VAR_ACCESS = { { POSITION, &variable }, + { VELOCITY, &variable }, + { ACCELERATION, + &variable } }; const char* errorCodeToMessage(int error_code) { diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index e55267689c..cd4b0ce5c6 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -1026,7 +1026,7 @@ void BenchmarkExecutor::collectMetrics(PlannerRunData& metrics, const robot_trajectory::RobotTrajectory& p = *motion_plan_response.trajectory[j]; // compute path length - traj_len = robot_trajectory::path_length(p); + traj_len = robot_trajectory::pathLength(p); // compute correctness and clearance collision_detection::CollisionRequest req; diff --git a/moveit_ros/move_group/src/move_group_capability.cpp b/moveit_ros/move_group/src/move_group_capability.cpp index 886076297e..d3861d8b9c 100644 --- a/moveit_ros/move_group/src/move_group_capability.cpp +++ b/moveit_ros/move_group/src/move_group_capability.cpp @@ -156,7 +156,7 @@ std::string move_group::MoveGroupCapability::getActionResultString(const moveit_ case moveit_msgs::msg::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE: return "Solution found but the environment changed during execution and the path was aborted"; default: - return moveit::core::error_code_to_string(error_code); + return moveit::core::errorCodeToString(error_code); } } diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp index e3bb61c4c8..21fd7c818f 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp @@ -52,7 +52,7 @@ namespace occupancy_map_monitor { namespace { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.occupancy_map_monitor"); +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.occupancy_map_monitor"); } OccupancyMapMonitor::OccupancyMapMonitor(const rclcpp::Node::SharedPtr& node, double map_resolution) diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor_middleware_handle.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor_middleware_handle.cpp index 4ba3e80daf..6f61c86fa1 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor_middleware_handle.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor_middleware_handle.cpp @@ -50,7 +50,7 @@ namespace occupancy_map_monitor { namespace { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.occupancy_map_monitor.middleware_handle"); +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.occupancy_map_monitor.middleware_handle"); } OccupancyMapMonitorMiddlewareHandle::OccupancyMapMonitorMiddlewareHandle(const rclcpp::Node::SharedPtr& node, diff --git a/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h index ba32c93bf5..5f75d59eb0 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h +++ b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h @@ -64,7 +64,6 @@ class DepthImageOctomapUpdater : public OccupancyMapUpdater void depthImageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& depth_msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info_msg); bool getShapeTransform(mesh_filter::MeshHandle h, Eigen::Isometry3d& transform) const; - void stopHelper(); rclcpp::Node::SharedPtr node_; std::shared_ptr tf_buffer_; 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 958bf35a6f..1cc05aac20 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 @@ -76,7 +76,7 @@ DepthImageOctomapUpdater::DepthImageOctomapUpdater() DepthImageOctomapUpdater::~DepthImageOctomapUpdater() { - stopHelper(); + sub_depth_image_.shutdown(); } bool DepthImageOctomapUpdater::setParams(const std::string& name_space) @@ -158,11 +158,6 @@ void DepthImageOctomapUpdater::start() } void DepthImageOctomapUpdater::stop() -{ - stopHelper(); -} - -void DepthImageOctomapUpdater::stopHelper() { sub_depth_image_.shutdown(); } @@ -208,19 +203,16 @@ bool DepthImageOctomapUpdater::getShapeTransform(mesh_filter::MeshHandle h, Eige namespace { -bool host_is_big_endian() -{ +const bool HOST_IS_BIG_ENDIAN = []() { union { uint32_t i; char c[sizeof(uint32_t)]; } bint = { 0x01020304 }; return bint.c[0] == 1; -} +}(); } // namespace -static const bool HOST_IS_BIG_ENDIAN = host_is_big_endian(); - void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& depth_msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info_msg) { diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h index cf33653249..3532389869 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h @@ -299,11 +299,11 @@ class GLRenderer float cy_; /** \brief map from thread id to OpenGL context */ - static std::map > context; + static std::map > s_context; /* \brief lock for context map */ - static std::mutex context_lock; + static std::mutex s_context_lock; - static bool glut_initialized; + static bool s_glut_initialized; }; } // namespace mesh_filter diff --git a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp index 2176b9db7d..543b341920 100644 --- a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp +++ b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp @@ -357,9 +357,9 @@ GLuint mesh_filter::GLRenderer::loadShaders(const string& vertex_source, const s return program_id; } -map > mesh_filter::GLRenderer::context; -std::mutex mesh_filter::GLRenderer::context_lock; -bool mesh_filter::GLRenderer::glut_initialized = false; +map > mesh_filter::GLRenderer::s_context; +std::mutex mesh_filter::GLRenderer::s_context_lock; +bool mesh_filter::GLRenderer::s_glut_initialized = false; namespace { @@ -370,8 +370,8 @@ void nullDisplayFunction() void mesh_filter::GLRenderer::createGLContext() { - std::unique_lock _(context_lock); - if (!glut_initialized) + std::unique_lock _(s_context_lock); + if (!s_glut_initialized) { char buffer[1]; char* args = buffer; @@ -379,16 +379,16 @@ void mesh_filter::GLRenderer::createGLContext() glutInit(&n, &args); glutInitDisplayMode(GLUT_SINGLE | GLUT_RGB | GLUT_DEPTH); - glut_initialized = true; + s_glut_initialized = true; } // check if our thread is initialized std::thread::id thread_id = std::this_thread::get_id(); - map >::iterator context_it = context.find(thread_id); + map >::iterator context_it = s_context.find(thread_id); - if (context_it == context.end()) + if (context_it == s_context.end()) { - context[thread_id] = std::pair(1, 0); + s_context.at(thread_id) = std::pair(1, 0); glutInitWindowPosition(glutGet(GLUT_SCREEN_WIDTH) + 30000, 0); glutInitWindowSize(1, 1); @@ -409,7 +409,7 @@ void mesh_filter::GLRenderer::createGLContext() for (int i = 0; i < 10; ++i) glutMainLoopEvent(); - context[thread_id] = std::pair(1, window_id); + s_context.at(thread_id) = std::pair(1, window_id); } else ++(context_it->second.first); @@ -417,10 +417,10 @@ void mesh_filter::GLRenderer::createGLContext() void mesh_filter::GLRenderer::deleteGLContext() { - std::unique_lock _(context_lock); + std::unique_lock _(s_context_lock); std::thread::id thread_id = std::this_thread::get_id(); - map >::iterator context_it = context.find(thread_id); - if (context_it == context.end()) + map >::iterator context_it = s_context.find(thread_id); + if (context_it == s_context.end()) { stringstream error_msg; error_msg << "No OpenGL context exists for Thread " << thread_id; @@ -430,7 +430,7 @@ void mesh_filter::GLRenderer::deleteGLContext() if (--(context_it->second.first) == 0) { glutDestroyWindow(context_it->second.second); - context.erase(context_it); + s_context.erase(context_it); } } diff --git a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h index 79f00bf86f..57512f959e 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h +++ b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h @@ -52,7 +52,7 @@ class PointCloudOctomapUpdater : public OccupancyMapUpdater { public: PointCloudOctomapUpdater(); - ~PointCloudOctomapUpdater() override; + ~PointCloudOctomapUpdater() override{}; bool setParams(const std::string& name_space) override; @@ -69,7 +69,6 @@ class PointCloudOctomapUpdater : public OccupancyMapUpdater private: bool getShapeTransform(ShapeHandle h, Eigen::Isometry3d& transform) const; void cloudMsgCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& cloud_msg); - void stopHelper(); // TODO: Enable private node for publishing filtered point cloud // ros::NodeHandle root_nh_; 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 a77cc7fa6c..2e34d4df8f 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 @@ -61,11 +61,6 @@ PointCloudOctomapUpdater::PointCloudOctomapUpdater() { } -PointCloudOctomapUpdater::~PointCloudOctomapUpdater() -{ - stopHelper(); -} - bool PointCloudOctomapUpdater::setParams(const std::string& name_space) { // This parameter is optional @@ -129,15 +124,10 @@ void PointCloudOctomapUpdater::start() } } -void PointCloudOctomapUpdater::stopHelper() +void PointCloudOctomapUpdater::stop() { delete point_cloud_filter_; delete point_cloud_subscriber_; -} - -void PointCloudOctomapUpdater::stop() -{ - stopHelper(); point_cloud_filter_ = nullptr; point_cloud_subscriber_ = nullptr; } diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index 378bd1ec82..39c938d92d 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -258,7 +258,7 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p else { RCLCPP_DEBUG(LOGGER, "PlanExecution terminating with error code %d - '%s'", plan.error_code.val, - moveit::core::error_code_to_string(plan.error_code).c_str()); + moveit::core::errorCodeToString(plan.error_code).c_str()); } } diff --git a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/solution_selection_functions.hpp b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/solution_selection_functions.hpp index 535a9aecc2..c53eccc2ff 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/solution_selection_functions.hpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/solution_selection_functions.hpp @@ -43,7 +43,7 @@ namespace moveit { namespace planning_pipeline_interfaces { -/** \brief Function that returns the shortest solution out of a vector of solutions based on robot_trajectory::path_length(...) +/** \brief Function that returns the shortest solution out of a vector of solutions based on robot_trajectory::pathLength(...) * \param [in] solutions Vector of solutions to chose the shortest one from * \return Shortest solution, trajectory of the returned MotionPlanResponse is a nullptr if no solution is found! */ diff --git a/moveit_ros/planning/planning_pipeline_interfaces/src/solution_selection_functions.cpp b/moveit_ros/planning/planning_pipeline_interfaces/src/solution_selection_functions.cpp index 121e2b4c52..d7d3b76fdb 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/src/solution_selection_functions.cpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/solution_selection_functions.cpp @@ -50,8 +50,8 @@ getShortestSolution(const std::vector<::planning_interface::MotionPlanResponse>& // If both solutions were successful, check which path is shorter if (solution_a && solution_b) { - return robot_trajectory::path_length(*solution_a.trajectory) < - robot_trajectory::path_length(*solution_b.trajectory); + return robot_trajectory::pathLength(*solution_a.trajectory) < + robot_trajectory::pathLength(*solution_b.trajectory); } // If only solution a is successful, return a else if (solution_a) diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp index ed792a0b91..538c2a3c63 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp @@ -50,7 +50,7 @@ using namespace std::chrono_literals; namespace { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.current_state_monitor"); +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.current_state_monitor"); } CurrentStateMonitor::CurrentStateMonitor(std::unique_ptr middleware_handle, 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 09e64e7db4..a69d9e2b1f 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 @@ -90,12 +90,12 @@ enum ActiveTargetType // Rolling has deprecated the version of the create_client method that takes // rmw_qos_profile_services_default for the QoS argument #if RCLCPP_VERSION_GTE(17, 0, 0) // Rolling -auto qos_default() +auto qosDefault() { return rclcpp::SystemDefaultsQoS(); } #else // Humble -auto qos_default() +auto qosDefault() { return rmw_qos_profile_services_default; } @@ -179,16 +179,16 @@ class MoveGroupInterface::MoveGroupInterfaceImpl execute_action_client_->wait_for_action_server(wait_for_servers.to_chrono>()); query_service_ = node_->create_client( - rclcpp::names::append(opt_.move_group_namespace, move_group::QUERY_PLANNERS_SERVICE_NAME), qos_default(), + rclcpp::names::append(opt_.move_group_namespace, move_group::QUERY_PLANNERS_SERVICE_NAME), qosDefault(), callback_group_); get_params_service_ = node_->create_client( - rclcpp::names::append(opt_.move_group_namespace, move_group::GET_PLANNER_PARAMS_SERVICE_NAME), qos_default(), + rclcpp::names::append(opt_.move_group_namespace, move_group::GET_PLANNER_PARAMS_SERVICE_NAME), qosDefault(), callback_group_); set_params_service_ = node_->create_client( - rclcpp::names::append(opt_.move_group_namespace, move_group::SET_PLANNER_PARAMS_SERVICE_NAME), qos_default(), + rclcpp::names::append(opt_.move_group_namespace, move_group::SET_PLANNER_PARAMS_SERVICE_NAME), qosDefault(), callback_group_); cartesian_path_service_ = node_->create_client( - rclcpp::names::append(opt_.move_group_namespace, move_group::CARTESIAN_PATH_SERVICE_NAME), qos_default(), + rclcpp::names::append(opt_.move_group_namespace, move_group::CARTESIAN_PATH_SERVICE_NAME), qosDefault(), callback_group_); RCLCPP_INFO_STREAM(LOGGER, "Ready to take commands for planning group " << opt.group_name << '.'); 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 fb2d4afea0..2e7fab9b89 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 @@ -57,7 +57,7 @@ namespace { -QString subframe_poses_to_qstring(const moveit::core::FixedTransformsMap& subframes) +QString subframePosesToQstring(const moveit::core::FixedTransformsMap& subframes) { QString status_text = "\nIt has the subframes '"; for (const auto& subframe : subframes) @@ -266,7 +266,7 @@ static QString decideStatusText(const collision_detection::CollisionEnv::ObjectC } if (!obj->subframe_poses_.empty()) { - status_text += subframe_poses_to_qstring(obj->subframe_poses_); + status_text += subframePosesToQstring(obj->subframe_poses_); } return status_text; } @@ -277,7 +277,7 @@ static QString decideStatusText(const moveit::core::AttachedBody* attached_body) QString::fromStdString(attached_body->getAttachedLinkName()) + "'."; if (!attached_body->getSubframes().empty()) { - status_text += subframe_poses_to_qstring(attached_body->getSubframes()); + status_text += subframePosesToQstring(attached_body->getSubframes()); } return status_text; } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_param_widget.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_param_widget.cpp index f20cbf545a..a20d1e5cb2 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_param_widget.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_param_widget.cpp @@ -73,14 +73,14 @@ void MotionPlanningParamWidget::setGroupName(const std::string& group_name) property_tree_model_ = nullptr; } -bool try_lexical_convert(const QString& value, long& lvalue) +bool tryLexicalConvert(const QString& value, long& lvalue) { bool ok; lvalue = value.toLong(&ok); return ok; } -bool try_lexical_convert(const QString& value, double& dvalue) +bool tryLexicalConvert(const QString& value, double& dvalue) { bool ok; dvalue = value.toDouble(&ok); @@ -101,11 +101,11 @@ rviz_common::properties::Property* MotionPlanningParamWidget::createPropertyTree long value_long; double value_double; - if (try_lexical_convert(value, value_long)) + if (tryLexicalConvert(value, value_long)) { new rviz_common::properties::IntProperty(key, value_long, QString(), root, SLOT(changedValue()), this); } - else if (try_lexical_convert(value, value_double)) + else if (tryLexicalConvert(value, value_double)) { new rviz_common::properties::FloatProperty(key, value_double, QString(), root, SLOT(changedValue()), this); }