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..5709f0b2aa 100644 --- a/moveit_core/planning_interface/src/planning_interface.cpp +++ b/moveit_core/planning_interface/src/planning_interface.cpp @@ -53,10 +53,10 @@ struct ActiveContexts std::set contexts_; }; -static ActiveContexts& getActiveContexts() +ActiveContexts& getActiveContexts() { - static ActiveContexts ac; - return ac; + static ActiveContexts s_ac; + return s_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/robot_state_jacobian_benchmark.cpp b/moveit_core/robot_state/test/robot_state_jacobian_benchmark.cpp index 82c96a20d8..3cd89dd69a 100644 --- a/moveit_core/robot_state/test/robot_state_jacobian_benchmark.cpp +++ b/moveit_core/robot_state/test/robot_state_jacobian_benchmark.cpp @@ -48,7 +48,7 @@ constexpr char TEST_ROBOT[] = "panda"; constexpr char TEST_GROUP[] = "panda_arm"; -static void BM_MoveItJacobian(benchmark::State& st) +static void bmMoveItJacobian(benchmark::State& st) { // Load a test robot model. const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(TEST_ROBOT); @@ -79,7 +79,7 @@ static void BM_MoveItJacobian(benchmark::State& st) } } -static void BM_KDLJacobian(benchmark::State& st) +static void bmKdlJacobian(benchmark::State& st) { const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(TEST_ROBOT); @@ -129,5 +129,5 @@ static void BM_KDLJacobian(benchmark::State& st) } } -BENCHMARK(BM_MoveItJacobian); -BENCHMARK(BM_KDLJacobian); +BENCHMARK(bmMoveItJacobian); +BENCHMARK(bmKdlJacobian); diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index fb481b828b..52e73adfcb 100644 --- a/moveit_core/robot_state/test/robot_state_test.cpp +++ b/moveit_core/robot_state/test/robot_state_test.cpp @@ -60,7 +60,7 @@ Eigen::VectorXd makeVector(const std::vector& values) } // Checks the validity of state.getJacobian() at the given 'joint_values' and 'joint_velocities'. -void CheckJacobian(moveit::core::RobotState& state, const moveit::core::JointModelGroup& joint_model_group, +void checkJacobian(moveit::core::RobotState& state, const moveit::core::JointModelGroup& joint_model_group, const Eigen::VectorXd& joint_values, const Eigen::VectorXd& joint_velocities) { // Using the Jacobian, compute the Cartesian velocity vector at which the end-effector would move, with the given @@ -811,8 +811,8 @@ TEST(getJacobian, RevoluteJoints) const moveit::core::JointModelGroup* jmg = state.getJointModelGroup("base_to_tip"); // Some made-up numbers, at zero and non-zero robot configurations. - CheckJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0, 0.0 }), makeVector({ 0.1, 0.2, 0.3, 0.4 })); - CheckJacobian(state, *jmg, makeVector({ 0.1, 0.2, 0.3, 0.4 }), makeVector({ 0.5, 0.3, 0.2, 0.1 })); + checkJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0, 0.0 }), makeVector({ 0.1, 0.2, 0.3, 0.4 })); + checkJacobian(state, *jmg, makeVector({ 0.1, 0.2, 0.3, 0.4 }), makeVector({ 0.5, 0.3, 0.2, 0.1 })); } TEST(getJacobian, RevoluteAndPrismaticJoints) @@ -879,8 +879,8 @@ TEST(getJacobian, RevoluteAndPrismaticJoints) const moveit::core::JointModelGroup* jmg = state.getJointModelGroup("base_to_tip"); // Some made-up numbers, at zero and non-zero robot configurations. - CheckJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0, 0.0 }), makeVector({ 0.1, 0.2, 0.3, 0.4 })); - CheckJacobian(state, *jmg, makeVector({ 0.1, 0.2, 0.3, 0.4 }), makeVector({ 0.5, 0.3, 0.2, 0.1 })); + checkJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0, 0.0 }), makeVector({ 0.1, 0.2, 0.3, 0.4 })); + checkJacobian(state, *jmg, makeVector({ 0.1, 0.2, 0.3, 0.4 }), makeVector({ 0.5, 0.3, 0.2, 0.1 })); } TEST(getJacobian, RevoluteAndFixedJoints) @@ -941,8 +941,8 @@ TEST(getJacobian, RevoluteAndFixedJoints) const moveit::core::JointModelGroup* jmg = state.getJointModelGroup("base_to_tip"); // Some made-up numbers, at zero and non-zero robot configurations. - CheckJacobian(state, *jmg, makeVector({ 0.0, 0.0 }), makeVector({ 0.1, 0.4 })); - CheckJacobian(state, *jmg, makeVector({ 0.1, 0.4 }), makeVector({ 0.5, 0.1 })); + checkJacobian(state, *jmg, makeVector({ 0.0, 0.0 }), makeVector({ 0.1, 0.4 })); + checkJacobian(state, *jmg, makeVector({ 0.1, 0.4 }), makeVector({ 0.5, 0.1 })); } TEST(getJacobian, RevolutePlanarAndPrismaticJoints) @@ -1008,7 +1008,7 @@ TEST(getJacobian, RevolutePlanarAndPrismaticJoints) moveit::core::RobotState state(robot_model); const moveit::core::JointModelGroup* jmg = state.getJointModelGroup("base_to_tip"); - CheckJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }), + checkJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }), makeVector({ 0.2, 0.05, 0.1, 0.2, 0.3, 0.4 })); } @@ -1073,8 +1073,8 @@ TEST(getJacobian, GroupNotAtOrigin) const moveit::core::JointModelGroup* jmg = state.getJointModelGroup("base_to_tip"); // Some made-up numbers, at zero and non-zero robot configurations. - CheckJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0 }), makeVector({ 0.1, 0.4, 0.2 })); - CheckJacobian(state, *jmg, makeVector({ 0.1, 0.4, 0.3 }), makeVector({ 0.5, 0.1, 0.2 })); + checkJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0 }), makeVector({ 0.1, 0.4, 0.2 })); + checkJacobian(state, *jmg, makeVector({ 0.1, 0.4, 0.3 }), makeVector({ 0.5, 0.1, 0.2 })); } int main(int argc, char** argv) diff --git a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp index 2245dc129d..03c30beb76 100644 --- a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp +++ b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp @@ -205,133 +205,132 @@ TEST_F(SimpleRobot, checkRelativeJointSpaceJump) EXPECT_NEAR(1.0, fraction, 0.01); } -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()); - } - - static void TearDownTestSuite() - { - 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_); - } - - double computeCartesianPath(std::vector>& result, const Eigen::Vector3d& translation, - bool global) - { - return CartesianInterpolator::computeCartesianPath(start_state_.get(), jmg_, result, link_, translation, global, - MaxEEFStep(0.1), JumpThreshold(), GroupStateValidityCallbackFn(), - kinematics::KinematicsQueryOptions()); - } - - 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, - 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_; -}; - // TODO - The tests below fail since no kinematic plugins are found. Move the tests to IK plugin package. - +// 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()); +// } +// +// static void TearDownTestSuite() +// { +// 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); +// } +// +// double computeCartesianPath(std::vector>& result, const Eigen::Vector3d& translation, +// bool global) +// { +// return CartesianInterpolator::computeCartesianPath(start_state.get(), jmg, result, link, translation, global, +// MaxEEFStep(0.1), JumpThreshold(), GroupStateValidityCallbackFn(), +// kinematics::KinematicsQueryOptions()); +// } +// +// 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, +// 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; +// +// const double prec_ = 1e-8; +// RobotStatePtr start_state_; +// Eigen::Isometry3d start_pose_; +// std::vector> result_; +// }; +// // 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 +338,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/logger.hpp b/moveit_core/utils/include/moveit/utils/logger.hpp index 7d3039f02b..fcd3e72b6d 100644 --- a/moveit_core/utils/include/moveit/utils/logger.hpp +++ b/moveit_core/utils/include/moveit/utils/logger.hpp @@ -43,15 +43,15 @@ namespace moveit { // Function for getting a reference to a logger object kept in a global variable. -// Use get_logger_mut to set the logger to a node logger. -const rclcpp::Logger& get_logger(); +// Use getLoggerMut to set the logger to a node logger. +const rclcpp::Logger& getLogger(); // Function for getting a child logger. In Humble this also creates a node. // Do not use this in place as it will create a new logger each time, // instead store it in the state of your class or method. -rclcpp::Logger make_child_logger(const std::string& name); +rclcpp::Logger makeChildLogger(const std::string& name); // Mutable access to global logger for setting to node logger. -rclcpp::Logger& get_logger_mut(); +rclcpp::Logger& getLoggerMut(); } // namespace moveit 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/logger.cpp b/moveit_core/utils/src/logger.cpp index a6a314f160..f444235d68 100644 --- a/moveit_core/utils/src/logger.cpp +++ b/moveit_core/utils/src/logger.cpp @@ -43,12 +43,12 @@ namespace moveit { -const rclcpp::Logger& get_logger() +const rclcpp::Logger& getLogger() { - return get_logger_mut(); + return getLoggerMut(); } -rclcpp::Logger make_child_logger(const std::string& name) +rclcpp::Logger makeChildLogger(const std::string& name) { // On versions of ROS older than Iron we need to create a node for each child logger // Remove once Humble is EOL @@ -60,15 +60,15 @@ rclcpp::Logger make_child_logger(const std::string& name) static std::unordered_map> child_nodes; if (child_nodes.find(name) == child_nodes.end()) { - std::string ns = get_logger().get_name(); + std::string ns = getLogger().get_name(); child_nodes[name] = std::make_shared(name, ns); } #endif - return get_logger_mut().get_child(name); + return getLoggerMut().get_child(name); } -rclcpp::Logger& get_logger_mut() +rclcpp::Logger& getLoggerMut() { static rclcpp::Logger logger = rclcpp::get_logger("moveit"); return logger; 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_core/utils/test/logger_dut.cpp b/moveit_core/utils/test/logger_dut.cpp index ca1525f1fc..01201d62b3 100644 --- a/moveit_core/utils/test/logger_dut.cpp +++ b/moveit_core/utils/test/logger_dut.cpp @@ -44,11 +44,11 @@ int main(int argc, char** argv) rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("dut_node"); // Set the moveit logger to be from node - moveit::get_logger_mut() = node->get_logger(); + moveit::getLoggerMut() = node->get_logger(); // A node logger, should be in the file output and rosout auto wall_timer = node->create_wall_timer(std::chrono::milliseconds(100), - [&] { RCLCPP_INFO(moveit::get_logger(), "hello from node!"); }); + [&] { RCLCPP_INFO(moveit::getLogger(), "hello from node!"); }); rclcpp::spin(node); } diff --git a/moveit_core/utils/test/logger_from_child_dut.cpp b/moveit_core/utils/test/logger_from_child_dut.cpp index a444d038b7..b051a98e30 100644 --- a/moveit_core/utils/test/logger_from_child_dut.cpp +++ b/moveit_core/utils/test/logger_from_child_dut.cpp @@ -44,10 +44,10 @@ int main(int argc, char** argv) rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("dut_node"); // Set the moveit logger to be from node - moveit::get_logger_mut() = node->get_logger(); + moveit::getLoggerMut() = node->get_logger(); // Make a child logger - const auto child_logger = moveit::make_child_logger("child"); + const auto child_logger = moveit::makeChildLogger("child"); // publish via a timer auto wall_timer = node->create_wall_timer(std::chrono::milliseconds(100), 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..89bd99f1da 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 declareParameterTemplate(const rclcpp::Node::SharedPtr& node, const std::string& name, T default_value) { if (not node->has_parameter(name)) { @@ -41,31 +41,31 @@ void declare_parameter(const rclcpp::Node::SharedPtr& node, const std::string& n namespace joint_limits { -inline bool declare_parameters(const std::string& joint_name, const rclcpp::Node::SharedPtr& node, - const std::string& param_ns) +inline bool declareParameters(const std::string& joint_name, const rclcpp::Node::SharedPtr& node, + const std::string& param_ns) { const std::string param_base_name = (param_ns.empty() ? "" : param_ns + ".") + "joint_limits." + joint_name; 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()); + declareParameterTemplate(node, param_base_name + ".has_position_limits", false); + declareParameterTemplate(node, param_base_name + ".min_position", std::numeric_limits::quiet_NaN()); + declareParameterTemplate(node, param_base_name + ".max_position", std::numeric_limits::quiet_NaN()); + declareParameterTemplate(node, param_base_name + ".has_velocity_limits", false); + declareParameterTemplate(node, param_base_name + ".min_velocity", std::numeric_limits::quiet_NaN()); + declareParameterTemplate(node, param_base_name + ".max_velocity", std::numeric_limits::quiet_NaN()); + declareParameterTemplate(node, param_base_name + ".has_acceleration_limits", false); + declareParameterTemplate(node, param_base_name + ".max_acceleration", std::numeric_limits::quiet_NaN()); + declareParameterTemplate(node, param_base_name + ".has_jerk_limits", false); + declareParameterTemplate(node, param_base_name + ".max_jerk", std::numeric_limits::quiet_NaN()); + declareParameterTemplate(node, param_base_name + ".has_effort_limits", false); + declareParameterTemplate(node, param_base_name + ".max_effort", std::numeric_limits::quiet_NaN()); + declareParameterTemplate(node, param_base_name + ".angle_wraparound", false); + declareParameterTemplate(node, param_base_name + ".has_soft_limits", false); + declareParameterTemplate(node, param_base_name + ".k_position", std::numeric_limits::quiet_NaN()); + declareParameterTemplate(node, param_base_name + ".k_velocity", std::numeric_limits::quiet_NaN()); + declareParameterTemplate(node, param_base_name + ".soft_lower_limit", std::numeric_limits::quiet_NaN()); + declareParameterTemplate(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..d73caaed60 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 @@ -44,21 +44,21 @@ namespace pilz_industrial_motion_planner namespace joint_limits_interface { /** - * @see joint_limits::declare_parameters(...) + * @see joint_limits::declareParameters(...) */ inline bool declareParameters(const std::string& joint_name, const std::string& param_ns, const rclcpp::Node::SharedPtr& node) { - return joint_limits::declare_parameters(joint_name, node, param_ns); + return joint_limits::declareParameters(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_py/src/moveit/core.cpp b/moveit_py/src/moveit/core.cpp index 4f4f6e58b4..652a442c01 100644 --- a/moveit_py/src/moveit/core.cpp +++ b/moveit_py/src/moveit/core.cpp @@ -58,19 +58,19 @@ PYBIND11_MODULE(core, m) options.disable_function_signatures(); // Construct module classes - moveit_py::bind_collision_detection::init_collision_request(m); - moveit_py::bind_collision_detection::init_collision_result(m); - moveit_py::bind_collision_detection::init_world(m); - moveit_py::bind_collision_detection::init_acm(m); - moveit_py::bind_controller_manager::init_execution_status(m); - moveit_py::bind_kinematic_constraints::init_kinematic_constraints(m); - moveit_py::bind_planning_scene::init_planning_scene(m); - moveit_py::bind_planning_interface::init_motion_plan_response(m); - moveit_py::bind_robot_model::init_joint_model(m); - moveit_py::bind_robot_model::init_joint_model_group(m); - moveit_py::bind_robot_model::init_robot_model(m); - moveit_py::bind_robot_state::init_robot_state(m); - moveit_py::bind_robot_trajectory::init_robot_trajectory(m); + moveit_py::bind_collision_detection::initCollisionRequest(m); + moveit_py::bind_collision_detection::initCollisionResult(m); + moveit_py::bind_collision_detection::initWorld(m); + moveit_py::bind_collision_detection::initAcm(m); + moveit_py::bind_controller_manager::initExecutionStatus(m); + moveit_py::bind_kinematic_constraints::initKinematicConstraints(m); + moveit_py::bind_planning_scene::initPlanningScene(m); + moveit_py::bind_planning_interface::initMotionPlanResponse(m); + moveit_py::bind_robot_model::initJointModel(m); + moveit_py::bind_robot_model::initJointModelGroup(m); + moveit_py::bind_robot_model::initRobotModel(m); + moveit_py::bind_robot_state::initRobotState(m); + moveit_py::bind_robot_trajectory::initRobotTrajectory(m); // TODO (peterdavidfagan): complete LinkModel bindings // LinkModel // py::class_(m, "LinkModel"); diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.cpp b/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.cpp index af2b9a6f7b..29082f9b3f 100644 --- a/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.cpp +++ b/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.cpp @@ -40,7 +40,7 @@ namespace moveit_py { namespace bind_collision_detection { -void init_collision_request(py::module& m) +void initCollisionRequest(py::module& m) { py::module collision_detection = m.def_submodule("collision_detection"); @@ -96,7 +96,7 @@ void init_collision_request(py::module& m) )"); } -void init_collision_result(py::module& m) +void initCollisionResult(py::module& m) { py::module collision_detection = m.def_submodule("collision_detection"); diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.h b/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.h index f3f8866cb0..63af366183 100644 --- a/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.h +++ b/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.h @@ -45,7 +45,7 @@ namespace moveit_py { namespace bind_collision_detection { -void init_collision_request(py::module& m); -void init_collision_result(py::module& m); +void initCollisionRequest(py::module& m); +void initCollisionResult(py::module& m); } // namespace bind_collision_detection } // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.cpp b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.cpp index 7c23bbb226..fb5434b8b1 100644 --- a/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.cpp +++ b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.cpp @@ -41,8 +41,8 @@ namespace moveit_py namespace bind_collision_detection { std::pair -get_entry(const std::shared_ptr& acm, const std::string& name1, - const std::string& name2) +getEntry(const std::shared_ptr& acm, const std::string& name1, + const std::string& name2) { // check acm for collision collision_detection::AllowedCollision::Type type; @@ -53,7 +53,7 @@ get_entry(const std::shared_ptr& ac return result; } -void init_acm(py::module& m) +void initAcm(py::module& m) { py::module collision_detection = m.def_submodule("collision_detection"); @@ -72,7 +72,7 @@ void init_acm(py::module& m) )", py::arg("names"), py::arg("default_entry") = false) - .def("get_entry", &moveit_py::bind_collision_detection::get_entry, + .def("get_entry", &moveit_py::bind_collision_detection::getEntry, R"( Get the allowed collision entry for a pair of objects. diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.h b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.h index 653096679d..68854c532d 100644 --- a/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.h +++ b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.h @@ -45,6 +45,6 @@ namespace moveit_py { namespace bind_collision_detection { -void init_acm(py::module& m); +void initAcm(py::module& m); } // namespace bind_collision_detection } // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/world.cpp b/moveit_py/src/moveit/moveit_core/collision_detection/world.cpp index c98489363f..ee2e69cc62 100644 --- a/moveit_py/src/moveit/moveit_core/collision_detection/world.cpp +++ b/moveit_py/src/moveit/moveit_core/collision_detection/world.cpp @@ -40,7 +40,7 @@ namespace moveit_py { namespace bind_collision_detection { -void init_world(py::module& m) +void initWorld(py::module& m) { py::module collision_detection = m.def_submodule("collision_detection"); diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/world.h b/moveit_py/src/moveit/moveit_core/collision_detection/world.h index 6cec992791..2dd0b8ef5b 100644 --- a/moveit_py/src/moveit/moveit_core/collision_detection/world.h +++ b/moveit_py/src/moveit/moveit_core/collision_detection/world.h @@ -45,6 +45,6 @@ namespace moveit_py { namespace bind_collision_detection { -void init_world(py::module& m); +void initWorld(py::module& m); } // namespace bind_collision_detection } // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.cpp b/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.cpp index 0fe70b43eb..140532462e 100644 --- a/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.cpp +++ b/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.cpp @@ -40,7 +40,7 @@ namespace moveit_py { namespace bind_controller_manager { -void init_execution_status(py::module& m) +void initExecutionStatus(py::module& m) { py::module controller_manager = m.def_submodule("controller_manager"); diff --git a/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.h b/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.h index f1df3681a7..2fa5390292 100644 --- a/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.h +++ b/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.h @@ -47,6 +47,6 @@ namespace moveit_py { namespace bind_controller_manager { -void init_execution_status(py::module& m); +void initExecutionStatus(py::module& m); } } // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp index b227128341..fa1ee2fea7 100644 --- a/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp +++ b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp @@ -43,11 +43,11 @@ namespace moveit_py { namespace bind_kinematic_constraints { -moveit_msgs::msg::Constraints construct_link_constraint(const std::string& link_name, const std::string& source_frame, - std::optional> cartesian_position, - std::optional cartesian_position_tolerance, - std::optional> orientation, - std::optional orientation_tolerance) +moveit_msgs::msg::Constraints constructLinkConstraint(const std::string& link_name, const std::string& source_frame, + std::optional> cartesian_position, + std::optional cartesian_position_tolerance, + std::optional> orientation, + std::optional orientation_tolerance) { // check that link cartesian and/or orientation constraints are specified if (!cartesian_position && !orientation) @@ -119,9 +119,9 @@ moveit_msgs::msg::Constraints construct_link_constraint(const std::string& link_ return constraints_cpp; } -moveit_msgs::msg::Constraints construct_joint_constraint(moveit::core::RobotState& robot_state, - moveit::core::JointModelGroup* joint_model_group, - double tolerance) +moveit_msgs::msg::Constraints constructJointConstraint(moveit::core::RobotState& robot_state, + moveit::core::JointModelGroup* joint_model_group, + double tolerance) { // generate joint constraint message moveit_msgs::msg::Constraints joint_constraints = @@ -130,8 +130,8 @@ moveit_msgs::msg::Constraints construct_joint_constraint(moveit::core::RobotStat return joint_constraints; } -moveit_msgs::msg::Constraints construct_constraints_from_node(const std::shared_ptr& node_name, - const std::string& ns) +moveit_msgs::msg::Constraints constructConstraintsFromNode(const std::shared_ptr& node_name, + const std::string& ns) { // construct constraint message moveit_msgs::msg::Constraints constraints_cpp; @@ -140,18 +140,18 @@ moveit_msgs::msg::Constraints construct_constraints_from_node(const std::shared_ return constraints_cpp; } -void init_kinematic_constraints(py::module& m) +void initKinematicConstraints(py::module& m) { py::module kinematic_constraints = m.def_submodule("kinematic_constraints"); - kinematic_constraints.def("construct_link_constraint", &construct_link_constraint, py::arg("link_name"), + kinematic_constraints.def("construct_link_constraint", &constructLinkConstraint, py::arg("link_name"), py::arg("source_frame"), py::arg("cartesian_position") = nullptr, py::arg("cartesian_position_tolerance") = nullptr, py::arg("orientation") = nullptr, py::arg("orientation_tolerance") = nullptr, "Construct a link constraint message"); - kinematic_constraints.def("construct_joint_constraint", &construct_joint_constraint, py::arg("robot_state"), + kinematic_constraints.def("construct_joint_constraint", &constructJointConstraint, py::arg("robot_state"), py::arg("joint_model_group"), py::arg("tolerance") = 0.01, "Construct a joint constraint message"); - kinematic_constraints.def("construct_constraints_from_node", &construct_constraints_from_node, py::arg("node_name"), + kinematic_constraints.def("construct_constraints_from_node", &constructConstraintsFromNode, py::arg("node_name"), py::arg("ns"), "Construct a constraint message from a node"); } diff --git a/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.h b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.h index d27238d4a4..161e3b03fa 100644 --- a/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.h +++ b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.h @@ -47,20 +47,20 @@ namespace moveit_py { namespace bind_kinematic_constraints { -moveit_msgs::msg::Constraints construct_link_constraint(const std::string& link_name, const std::string& source_frame, - std::optional> cartesian_position, - std::optional cartesian_position_tolerance, - std::optional> orientation, - std::optional orientation_tolerance); +moveit_msgs::msg::Constraints constructLinkConstraint(const std::string& link_name, const std::string& source_frame, + std::optional> cartesian_position, + std::optional cartesian_position_tolerance, + std::optional> orientation, + std::optional orientation_tolerance); -moveit_msgs::msg::Constraints construct_joint_constraint(moveit::core::RobotState& robot_state, - moveit::core::JointModelGroup* joint_model_group, - double tolerance); +moveit_msgs::msg::Constraints constructJointConstraint(moveit::core::RobotState& robot_state, + moveit::core::JointModelGroup* joint_model_group, + double tolerance); -moveit_msgs::msg::Constraints construct_constraints_from_node(const std::shared_ptr& node_name, - const std::string& ns); +moveit_msgs::msg::Constraints constructConstraintsFromNode(const std::shared_ptr& node_name, + const std::string& ns); -void init_kinematic_constraints(py::module& m); +void initKinematicConstraints(py::module& m); } // namespace bind_kinematic_constraints } // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.cpp b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.cpp index 481f8a4eab..8ebf5437f4 100644 --- a/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.cpp +++ b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.cpp @@ -41,37 +41,37 @@ namespace moveit_py namespace bind_planning_interface { std::shared_ptr -get_motion_plan_response_trajectory(std::shared_ptr& response) +getMotionPlanResponseTrajectory(std::shared_ptr& response) { return response->trajectory; } moveit_msgs::msg::RobotState -get_motion_plan_response_start_state(std::shared_ptr& response) +getMotionPlanResponseStartState(std::shared_ptr& response) { moveit_msgs::msg::RobotState robot_state_msg = response->start_state; return robot_state_msg; } moveit_msgs::msg::MoveItErrorCodes -get_motion_plan_response_error_code(std::shared_ptr& response) +getMotionPlanResponseErrorCode(std::shared_ptr& response) { moveit_msgs::msg::MoveItErrorCodes error_code_msg = static_cast(response->error_code); return error_code_msg; } -double get_motion_plan_response_planning_time(std::shared_ptr& response) +double getMotionPlanResponsePlanningTime(std::shared_ptr& response) { return response->planning_time; } -std::string get_motion_plan_response_planner_id(std::shared_ptr& response) +std::string getMotionPlanResponsePlannerId(std::shared_ptr& response) { return response->planner_id; } -void init_motion_plan_response(py::module& m) +void initMotionPlanResponse(py::module& m) { py::module planning_interface = m.def_submodule("planning_interface"); @@ -80,16 +80,16 @@ void init_motion_plan_response(py::module& m) //.def(py::init<>(), R"()") - .def_property("trajectory", &moveit_py::bind_planning_interface::get_motion_plan_response_trajectory, nullptr, + .def_property("trajectory", &moveit_py::bind_planning_interface::getMotionPlanResponseTrajectory, nullptr, py::return_value_policy::copy, R"()") .def_readonly("planning_time", &planning_interface::MotionPlanResponse::planning_time, py::return_value_policy::copy, R"()") - .def_property("error_code", &moveit_py::bind_planning_interface::get_motion_plan_response_error_code, nullptr, + .def_property("error_code", &moveit_py::bind_planning_interface::getMotionPlanResponseErrorCode, nullptr, py::return_value_policy::copy, R"()") - .def_property("start_state", &moveit_py::bind_planning_interface::get_motion_plan_response_start_state, nullptr, + .def_property("start_state", &moveit_py::bind_planning_interface::getMotionPlanResponseStartState, nullptr, py::return_value_policy::copy, R"()") .def_readonly("planner_id", &planning_interface::MotionPlanResponse::planner_id, py::return_value_policy::copy, diff --git a/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.h b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.h index ebb408ab70..88943db5d6 100644 --- a/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.h +++ b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.h @@ -51,18 +51,18 @@ namespace moveit_py namespace bind_planning_interface { std::shared_ptr -get_motion_plan_response_trajectory(std::shared_ptr& response); +getMotionPlanResponseTrajectory(std::shared_ptr& response); moveit_msgs::msg::RobotState -get_motion_plan_response_start_state(std::shared_ptr& response); +getMotionPlanResponseStartState(std::shared_ptr& response); moveit_msgs::msg::MoveItErrorCodes -get_motion_plan_response_error_code(std::shared_ptr& response); +getMotionPlanResponseErrorCode(std::shared_ptr& response); -double get_motion_plan_response_planning_time(std::shared_ptr& response); +double getMotionPlanResponsePlanningTime(std::shared_ptr& response); -std::string get_motion_plan_response_planner_id(std::shared_ptr& response); +std::string getMotionPlanResponsePlannerId(std::shared_ptr& response); -void init_motion_plan_response(py::module& m); +void initMotionPlanResponse(py::module& m); } // namespace bind_planning_interface } // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp index 4caff8f7f2..22cef42453 100644 --- a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp +++ b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp @@ -42,9 +42,9 @@ namespace moveit_py { namespace bind_planning_scene { -void apply_collision_object(std::shared_ptr& planning_scene, - moveit_msgs::msg::CollisionObject& collision_object_msg, - std::optional color_msg) +void applyCollisionObject(std::shared_ptr& planning_scene, + moveit_msgs::msg::CollisionObject& collision_object_msg, + std::optional color_msg) { // apply collision object planning_scene->processCollisionObjectMsg(collision_object_msg); @@ -57,21 +57,20 @@ void apply_collision_object(std::shared_ptr& plan } } -Eigen::MatrixXd get_frame_transform(std::shared_ptr& planning_scene, - const std::string& id) +Eigen::MatrixXd getFrameTransform(std::shared_ptr& planning_scene, const std::string& id) { auto transformation = planning_scene->getFrameTransform(id); return transformation.matrix(); } -moveit_msgs::msg::PlanningScene get_planning_scene_msg(std::shared_ptr& planning_scene) +moveit_msgs::msg::PlanningScene getPlanningSceneMsg(std::shared_ptr& planning_scene) { moveit_msgs::msg::PlanningScene planning_scene_msg; planning_scene->getPlanningSceneMsg(planning_scene_msg); return planning_scene_msg; } -void init_planning_scene(py::module& m) +void initPlanningScene(py::module& m) { py::module planning_scene = m.def_submodule("planning_scene"); @@ -108,7 +107,7 @@ void init_planning_scene(py::module& m) :py:class:`moveit_py.core.RobotState`: The current state of the robot. )") - .def_property("planning_scene_message", &moveit_py::bind_planning_scene::get_planning_scene_msg, nullptr, + .def_property("planning_scene_message", &moveit_py::bind_planning_scene::getPlanningSceneMsg, nullptr, py::return_value_policy::move) .def_property("transforms", py::overload_cast<>(&planning_scene::PlanningScene::getTransforms), nullptr) @@ -151,7 +150,7 @@ void init_planning_scene(py::module& m) bool: True if the transform is known, false otherwise. )") - .def("get_frame_transform", &moveit_py::bind_planning_scene::get_frame_transform, py::arg("frame_id"), + .def("get_frame_transform", &moveit_py::bind_planning_scene::getFrameTransform, py::arg("frame_id"), R"( Get the transform corresponding to the frame id. This will be known if id is a link name, an attached body id or a collision object. Return identity when no transform is available. @@ -172,7 +171,7 @@ void init_planning_scene(py::module& m) msg (:py:class:`moveit_msgs.msg.PlanningSceneWorld`): The planning scene world message. )") - .def("apply_collision_object", &moveit_py::bind_planning_scene::apply_collision_object, + .def("apply_collision_object", &moveit_py::bind_planning_scene::applyCollisionObject, py::arg("collision_object_msg"), py::arg("color_msg") = nullptr, R"( Apply a collision object to the planning scene. diff --git a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.h b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.h index b9ec922278..14a6d1ca1d 100644 --- a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.h +++ b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.h @@ -49,15 +49,14 @@ namespace moveit_py { namespace bind_planning_scene { -void apply_collision_object(std::shared_ptr& planning_scene, - moveit_msgs::msg::CollisionObject& collision_object_msg, - std::optional color_msg); +void applyCollisionObject(std::shared_ptr& planning_scene, + moveit_msgs::msg::CollisionObject& collision_object_msg, + std::optional color_msg); -Eigen::MatrixXd get_frame_transform(std::shared_ptr& planning_scene, - const std::string& id); +Eigen::MatrixXd getFrameTransform(std::shared_ptr& planning_scene, const std::string& id); -moveit_msgs::msg::PlanningScene get_planning_scene_msg(std::shared_ptr& planning_scene); +moveit_msgs::msg::PlanningScene getPlanningSceneMsg(std::shared_ptr& planning_scene); -void init_planning_scene(py::module& m); +void initPlanningScene(py::module& m); } // namespace bind_planning_scene } // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/robot_model/joint_model.cpp b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.cpp index 8132615eaa..7d335b0b7f 100644 --- a/moveit_py/src/moveit/moveit_core/robot_model/joint_model.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.cpp @@ -42,7 +42,7 @@ namespace moveit_py namespace bind_robot_model { -void init_joint_model(py::module& m) +void initJointModel(py::module& m) { py::module robot_model = m.def_submodule("robot_model"); diff --git a/moveit_py/src/moveit/moveit_core/robot_model/joint_model.h b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.h index a2a70ef6d5..bcd72d0171 100644 --- a/moveit_py/src/moveit/moveit_core/robot_model/joint_model.h +++ b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.h @@ -47,6 +47,6 @@ namespace moveit_py { namespace bind_robot_model { -void init_joint_model(py::module& m); +void initJointModel(py::module& m); } // namespace bind_robot_model } // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.cpp b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.cpp index e57f54d925..7d51171047 100644 --- a/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.cpp @@ -41,14 +41,14 @@ namespace moveit_py { namespace bind_robot_model { -bool satisfies_position_bounds(const moveit::core::JointModelGroup* jmg, const Eigen::VectorXd& joint_positions, - const double margin) +bool satisfiesPositionBounds(const moveit::core::JointModelGroup* jmg, const Eigen::VectorXd& joint_positions, + const double margin) { assert(joint_positions.size() == jmg->getActiveVariableCount()); return jmg->satisfiesPositionBounds(joint_positions.data(), margin); } -void init_joint_model_group(py::module& m) +void initJointModelGroup(py::module& m) { py::module robot_model = m.def_submodule("robot_model"); @@ -79,7 +79,7 @@ void init_joint_model_group(py::module& m) } return eef->getName(); }) - .def("satisfies_position_bounds", &moveit_py::bind_robot_model::satisfies_position_bounds, py::arg("values"), + .def("satisfies_position_bounds", &moveit_py::bind_robot_model::satisfiesPositionBounds, py::arg("values"), py::arg("margin") = 0.0); } } // namespace bind_robot_model diff --git a/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.h b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.h index a86358802b..be54b09e6a 100644 --- a/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.h +++ b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.h @@ -47,8 +47,8 @@ namespace moveit_py { namespace bind_robot_model { -bool satisfies_position_bounds(const moveit::core::JointModelGroup* jmg, const Eigen::VectorXd& joint_positions, - const double margin); -void init_joint_model_group(py::module& m); +bool satisfiesPositionBounds(const moveit::core::JointModelGroup* jmg, const Eigen::VectorXd& joint_positions, + const double margin); +void initJointModelGroup(py::module& m); } // namespace bind_robot_model } // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/robot_model/robot_model.cpp b/moveit_py/src/moveit/moveit_core/robot_model/robot_model.cpp index e689041b13..314cc06b06 100644 --- a/moveit_py/src/moveit/moveit_core/robot_model/robot_model.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_model/robot_model.cpp @@ -45,7 +45,7 @@ namespace moveit_py { namespace bind_robot_model { -void init_robot_model(py::module& m) +void initRobotModel(py::module& m) { py::module robot_model = m.def_submodule("robot_model"); diff --git a/moveit_py/src/moveit/moveit_core/robot_model/robot_model.h b/moveit_py/src/moveit/moveit_core/robot_model/robot_model.h index dd79de0608..9b5e8589f6 100644 --- a/moveit_py/src/moveit/moveit_core/robot_model/robot_model.h +++ b/moveit_py/src/moveit/moveit_core/robot_model/robot_model.h @@ -46,6 +46,6 @@ namespace moveit_py { namespace bind_robot_model { -void init_robot_model(py::module& m); +void initRobotModel(py::module& m); } // namespace bind_robot_model } // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp index 21294fbe36..ed97ef08b6 100644 --- a/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp @@ -64,25 +64,25 @@ void update(moveit::core::RobotState* self, bool force, std::string& category) } } -Eigen::MatrixXd get_frame_transform(const moveit::core::RobotState* self, std::string& frame_id) +Eigen::MatrixXd getFrameTransform(const moveit::core::RobotState* self, std::string& frame_id) { bool frame_found; auto transformation = self->getFrameTransform(frame_id, &frame_found); return transformation.matrix(); } -Eigen::MatrixXd get_global_link_transform(const moveit::core::RobotState* self, std::string& link_name) +Eigen::MatrixXd getGlobalLinkTransform(const moveit::core::RobotState* self, std::string& link_name) { auto transformation = self->getGlobalLinkTransform(link_name); return transformation.matrix(); } -geometry_msgs::msg::Pose get_pose(const moveit::core::RobotState* self, const std::string& link_name) +geometry_msgs::msg::Pose getPose(const moveit::core::RobotState* self, const std::string& link_name) { return tf2::toMsg(self->getGlobalLinkTransform(link_name)); } -std::map get_joint_positions(const moveit::core::RobotState* self) +std::map getJointPositions(const moveit::core::RobotState* self) { std::map joint_positions; const std::vector& variable_name = self->getVariableNames(); @@ -93,7 +93,7 @@ std::map get_joint_positions(const moveit::core::RobotState return joint_positions; } -void set_joint_positions(moveit::core::RobotState* self, std::map& joint_positions) +void setJointPositions(moveit::core::RobotState* self, std::map& joint_positions) { for (const auto& item : joint_positions) { @@ -101,7 +101,7 @@ void set_joint_positions(moveit::core::RobotState* self, std::map get_joint_velocities(const moveit::core::RobotState* self) +std::map getJointVelocities(const moveit::core::RobotState* self) { std::map joint_velocity; const std::vector& variable_name = self->getVariableNames(); @@ -112,7 +112,7 @@ std::map get_joint_velocities(const moveit::core::RobotStat return joint_velocity; } -void set_joint_velocities(moveit::core::RobotState* self, std::map& joint_velocities) +void setJointVelocities(moveit::core::RobotState* self, std::map& joint_velocities) { for (const auto& item : joint_velocities) { @@ -120,7 +120,7 @@ void set_joint_velocities(moveit::core::RobotState* self, std::map get_joint_accelerations(const moveit::core::RobotState* self) +std::map getJointAccelerations(const moveit::core::RobotState* self) { std::map joint_acceleration; const std::vector& variable_name = self->getVariableNames(); @@ -131,7 +131,7 @@ std::map get_joint_accelerations(const moveit::core::RobotS return joint_acceleration; } -void set_joint_accelerations(moveit::core::RobotState* self, std::map& joint_accelerations) +void setJointAccelerations(moveit::core::RobotState* self, std::map& joint_accelerations) { for (const auto& item : joint_accelerations) { @@ -139,7 +139,7 @@ void set_joint_accelerations(moveit::core::RobotState* self, std::map get_joint_efforts(const moveit::core::RobotState* self) +std::map getJointEfforts(const moveit::core::RobotState* self) { std::map joint_effort; const std::vector& variable_name = self->getVariableNames(); @@ -150,7 +150,7 @@ std::map get_joint_efforts(const moveit::core::RobotState* return joint_effort; } -void set_joint_efforts(moveit::core::RobotState* self, std::map& joint_efforts) +void setJointEfforts(moveit::core::RobotState* self, std::map& joint_efforts) { for (const auto& item : joint_efforts) { @@ -158,40 +158,38 @@ void set_joint_efforts(moveit::core::RobotState* self, std::mapcopyJointGroupPositions(joint_model_group_name, values); return values; } -Eigen::VectorXd copy_joint_group_velocities(const moveit::core::RobotState* self, - const std::string& joint_model_group_name) +Eigen::VectorXd copyJointGroupVelocities(const moveit::core::RobotState* self, const std::string& joint_model_group_name) { Eigen::VectorXd values; self->copyJointGroupVelocities(joint_model_group_name, values); return values; } -Eigen::VectorXd copy_joint_group_accelerations(const moveit::core::RobotState* self, - const std::string& joint_model_group_name) +Eigen::VectorXd copyJointGroupAccelerations(const moveit::core::RobotState* self, + const std::string& joint_model_group_name) { Eigen::VectorXd values; self->copyJointGroupAccelerations(joint_model_group_name, values); return values; } -Eigen::MatrixXd get_jacobian(const moveit::core::RobotState* self, const std::string& joint_model_group_name, - const Eigen::Vector3d& reference_point_position) +Eigen::MatrixXd getJacobian(const moveit::core::RobotState* self, const std::string& joint_model_group_name, + const Eigen::Vector3d& reference_point_position) { const moveit::core::JointModelGroup* joint_model_group = self->getJointModelGroup(joint_model_group_name); return self->getJacobian(joint_model_group, reference_point_position); } -Eigen::MatrixXd get_jacobian(const moveit::core::RobotState* self, const std::string& joint_model_group_name, - const std::string& link_model_name, const Eigen::Vector3d& reference_point_position, - bool use_quaternion_representation) +Eigen::MatrixXd getJacobian(const moveit::core::RobotState* self, const std::string& joint_model_group_name, + const std::string& link_model_name, const Eigen::Vector3d& reference_point_position, + bool use_quaternion_representation) { Eigen::MatrixXd jacobian; const moveit::core::JointModelGroup* joint_model_group = self->getJointModelGroup(joint_model_group_name); @@ -200,15 +198,15 @@ Eigen::MatrixXd get_jacobian(const moveit::core::RobotState* self, const std::st return jacobian; } -bool set_to_default_values(moveit::core::RobotState* self, const std::string& joint_model_group_name, - const std::string& state_name) +bool setToDefaultValues(moveit::core::RobotState* self, const std::string& joint_model_group_name, + const std::string& state_name) { const moveit::core::JointModelGroup* joint_model_group = self->getJointModelGroup(joint_model_group_name); return self->setToDefaultValues(joint_model_group, state_name); } -void init_robot_state(py::module& m) +void initRobotState(py::module& m) { py::module robot_state = m.def_submodule("robot_state"); @@ -250,10 +248,10 @@ void init_robot_state(py::module& m) bool: True if the robot state is dirty. )") - .def("get_frame_transform", &moveit_py::bind_robot_state::get_frame_transform, py::arg("frame_id"), + .def("get_frame_transform", &moveit_py::bind_robot_state::getFrameTransform, py::arg("frame_id"), py::return_value_policy::move, R"( - Get the transformation matrix from the model frame (root of model) to the frame identified by frame_id. If frame_id was not found, frame_found is set to false and an identity transform is returned. This method is restricted to frames defined within the robot state and doesn't include collision object present in the collision world. Please use the PlanningScene.get_frame_transform method for collision world objects. + Get the transformation matrix from the model frame (root of model) to the frame identified by frame_id. If frame_id was not found, frame_found is set to false and an identity transform is returned. This method is restricted to frames defined within the robot state and doesn't include collision object present in the collision world. Please use the PlanningScene.getFrameTransform method for collision world objects. Args: frame_id (str): The id of the frame to get the transform for. @@ -262,7 +260,7 @@ void init_robot_state(py::module& m) :py:class:`numpy.ndarray`: The transformation matrix from the model frame to the frame identified by frame_id. )") - .def("get_pose", &moveit_py::bind_robot_state::get_pose, py::arg("link_name"), + .def("get_pose", &moveit_py::bind_robot_state::getPose, py::arg("link_name"), R"( Get the pose of a link that is defined in the robot model. @@ -275,7 +273,7 @@ void init_robot_state(py::module& m) .def("get_jacobian", py::overload_cast( - &moveit_py::bind_robot_state::get_jacobian), + &moveit_py::bind_robot_state::getJacobian), py::arg("joint_model_group_name"), py::arg("reference_point_position"), py::return_value_policy::move, R"( Compute the Jacobian with reference to the last link of a specified group. @@ -293,7 +291,7 @@ void init_robot_state(py::module& m) .def("get_jacobian", py::overload_cast(&moveit_py::bind_robot_state::get_jacobian), + const Eigen::Vector3d&, bool>(&moveit_py::bind_robot_state::getJacobian), py::arg("joint_model_group_name"), py::arg("link_name"), py::arg("reference_point_position"), py::arg("use_quaternion_representation") = false, py::return_value_policy::move, R"( @@ -329,17 +327,17 @@ void init_robot_state(py::module& m) )") // Getting and setting joint model group positions, velocities, accelerations - .def_property("joint_positions", &moveit_py::bind_robot_state::get_joint_positions, - &moveit_py::bind_robot_state::set_joint_positions, py::return_value_policy::copy) + .def_property("joint_positions", &moveit_py::bind_robot_state::getJointPositions, + &moveit_py::bind_robot_state::setJointPositions, py::return_value_policy::copy) - .def_property("joint_velocities", &moveit_py::bind_robot_state::get_joint_velocities, - &moveit_py::bind_robot_state::set_joint_velocities, py::return_value_policy::copy) + .def_property("joint_velocities", &moveit_py::bind_robot_state::getJointVelocities, + &moveit_py::bind_robot_state::setJointVelocities, py::return_value_policy::copy) - .def_property("joint_accelerations", &moveit_py::bind_robot_state::get_joint_accelerations, - &moveit_py::bind_robot_state::set_joint_accelerations, py::return_value_policy::copy) + .def_property("joint_accelerations", &moveit_py::bind_robot_state::getJointAccelerations, + &moveit_py::bind_robot_state::setJointAccelerations, py::return_value_policy::copy) - .def_property("joint_efforts", &moveit_py::bind_robot_state::get_joint_efforts, - &moveit_py::bind_robot_state::set_joint_efforts, py::return_value_policy::copy) + .def_property("joint_efforts", &moveit_py::bind_robot_state::getJointEfforts, + &moveit_py::bind_robot_state::setJointEfforts, py::return_value_policy::copy) .def("set_joint_group_positions", py::overload_cast( @@ -366,7 +364,7 @@ void init_robot_state(py::module& m) position_values (:py:class:`numpy.ndarray`): The positions of the joints in the joint model group. )") - .def("get_joint_group_positions", &moveit_py::bind_robot_state::copy_joint_group_positions, + .def("get_joint_group_positions", &moveit_py::bind_robot_state::copyJointGroupPositions, py::arg("joint_model_group_name"), R"( For a given group, get the position values of the variables that make up the group. @@ -390,7 +388,7 @@ void init_robot_state(py::module& m) velocity_values (:py:class:`numpy.ndarray`): The velocities of the joints in the joint model group. )") - .def("get_joint_group_velocities", &moveit_py::bind_robot_state::copy_joint_group_velocities, + .def("get_joint_group_velocities", &moveit_py::bind_robot_state::copyJointGroupVelocities, py::arg("joint_model_group_name"), R"( For a given group, get the velocity values of the variables that make up the group. @@ -413,7 +411,7 @@ void init_robot_state(py::module& m) acceleration_values (:py:class:`numpy.ndarray`): The accelerations of the joints in the joint model group. )") - .def("get_joint_group_accelerations", &moveit_py::bind_robot_state::copy_joint_group_accelerations, + .def("get_joint_group_accelerations", &moveit_py::bind_robot_state::copyJointGroupAccelerations, py::arg("joint_model_group_name"), R"( For a given group, get the acceleration values of the variables that make up the group. @@ -426,7 +424,7 @@ void init_robot_state(py::module& m) )") // Forward kinematics - .def("get_global_link_transform", &moveit_py::bind_robot_state::get_global_link_transform, py::arg("link_name"), + .def("get_global_link_transform", &moveit_py::bind_robot_state::getGlobalLinkTransform, py::arg("link_name"), R"( Returns the transform of the specified link in the global frame. @@ -475,7 +473,7 @@ void init_robot_state(py::module& m) .def("set_to_default_values", py::overload_cast( - &moveit_py::bind_robot_state::set_to_default_values), + &moveit_py::bind_robot_state::setToDefaultValues), py::arg("joint_model_group_name"), py::arg("name"), R"( Set the joints in group to the position name defined in the SRDF. diff --git a/moveit_py/src/moveit/moveit_core/robot_state/robot_state.h b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.h index 01819d6918..e688705898 100644 --- a/moveit_py/src/moveit/moveit_core/robot_state/robot_state.h +++ b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.h @@ -51,29 +51,28 @@ namespace bind_robot_state { void update(moveit::core::RobotState* self, bool force, std::string& category); -Eigen::MatrixXd get_frame_transform(const moveit::core::RobotState* self, std::string& frame_id); +Eigen::MatrixXd getFrameTransform(const moveit::core::RobotState* self, std::string& frame_id); -Eigen::MatrixXd get_global_link_transform(const moveit::core::RobotState* self, std::string& link_name); +Eigen::MatrixXd getGlobalLinkTransform(const moveit::core::RobotState* self, std::string& link_name); -geometry_msgs::msg::Pose get_pose(const moveit::core::RobotState* self, const std::string& link_name); +geometry_msgs::msg::Pose getPose(const moveit::core::RobotState* self, const std::string& link_name); -Eigen::VectorXd copy_joint_group_positions(const moveit::core::RobotState* self, - const std::string& joint_model_group_name); -Eigen::VectorXd copy_joint_group_velocities(const moveit::core::RobotState* self, +Eigen::VectorXd copyJointGroupPositions(const moveit::core::RobotState* self, const std::string& joint_model_group_name); +Eigen::VectorXd copyJointGroupVelocities(const moveit::core::RobotState* self, + const std::string& joint_model_group_name); +Eigen::VectorXd copyJointGroupAccelerations(const moveit::core::RobotState* self, const std::string& joint_model_group_name); -Eigen::VectorXd copy_joint_group_accelerations(const moveit::core::RobotState* self, - const std::string& joint_model_group_name); -Eigen::MatrixXd get_jacobian(const moveit::core::RobotState* self, const std::string& joint_model_group_name, - const std::string& link_model_name, const Eigen::Vector3d& reference_point_position, - bool use_quaternion_representation); +Eigen::MatrixXd getJacobian(const moveit::core::RobotState* self, const std::string& joint_model_group_name, + const std::string& link_model_name, const Eigen::Vector3d& reference_point_position, + bool use_quaternion_representation); -Eigen::MatrixXd get_jacobian(const moveit::core::RobotState* self, const std::string& joint_model_group_name, - const Eigen::Vector3d& reference_point_position); +Eigen::MatrixXd getJacobian(const moveit::core::RobotState* self, const std::string& joint_model_group_name, + const Eigen::Vector3d& reference_point_position); -bool set_to_default_values(moveit::core::RobotState* self, const std::string& joint_model_group_name, - const std::string& state_name); +bool setToDefaultValues(moveit::core::RobotState* self, const std::string& joint_model_group_name, + const std::string& state_name); -void init_robot_state(py::module& m); +void initRobotState(py::module& m); } // namespace bind_robot_state } // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp index 183938e3ae..a18c121c96 100644 --- a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp @@ -43,8 +43,8 @@ namespace moveit_py namespace bind_robot_trajectory { moveit_msgs::msg::RobotTrajectory -get_robot_trajectory_msg(const robot_trajectory::RobotTrajectoryConstPtr& robot_trajectory, - const std::vector& joint_filter) +getRobotTrajectoryMsg(const robot_trajectory::RobotTrajectoryConstPtr& robot_trajectory, + const std::vector& joint_filter) { moveit_msgs::msg::RobotTrajectory msg; robot_trajectory->getRobotTrajectoryMsg(msg, joint_filter); @@ -52,13 +52,13 @@ get_robot_trajectory_msg(const robot_trajectory::RobotTrajectoryConstPtr& robot_ } robot_trajectory::RobotTrajectory -set_robot_trajectory_msg(const std::shared_ptr& robot_trajectory, - const moveit::core::RobotState& robot_state, const moveit_msgs::msg::RobotTrajectory& msg) +setRobotTrajectoryMsg(const std::shared_ptr& robot_trajectory, + const moveit::core::RobotState& robot_state, const moveit_msgs::msg::RobotTrajectory& msg) { return robot_trajectory->setRobotTrajectoryMsg(robot_state, msg); } -void init_robot_trajectory(py::module& m) +void initRobotTrajectory(py::module& m) { py::module robot_trajectory = m.def_submodule("robot_trajectory"); @@ -164,7 +164,7 @@ void init_robot_trajectory(py::module& m) Returns: bool: True if the trajectory was successfully retimed, false otherwise. )") - .def("get_robot_trajectory_msg", &moveit_py::bind_robot_trajectory::get_robot_trajectory_msg, + .def("get_robot_trajectory_msg", &moveit_py::bind_robot_trajectory::getRobotTrajectoryMsg, py::arg("joint_filter") = std::vector(), R"( Get the trajectory as a moveit_msgs.msg.RobotTrajectory message. @@ -174,8 +174,8 @@ void init_robot_trajectory(py::module& m) Returns: moveit_msgs.msg.RobotTrajectory: A ROS robot trajectory message. )") - .def("set_robot_trajectory_msg", &moveit_py::bind_robot_trajectory::set_robot_trajectory_msg, - py::arg("robot_state"), py::arg("msg"), + .def("set_robot_trajectory_msg", &moveit_py::bind_robot_trajectory::setRobotTrajectoryMsg, py::arg("robot_state"), + py::arg("msg"), R"( Set the trajectory from a moveit_msgs.msg.RobotTrajectory message. diff --git a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h index 29eb51b3cd..7e1cca5ace 100644 --- a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h +++ b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h @@ -50,12 +50,12 @@ namespace moveit_py namespace bind_robot_trajectory { moveit_msgs::msg::RobotTrajectory -get_robot_trajectory_msg(const robot_trajectory::RobotTrajectoryConstPtr& robot_trajectory, - const std::vector& joint_filter); +getRobotTrajectoryMsg(const robot_trajectory::RobotTrajectoryConstPtr& robot_trajectory, + const std::vector& joint_filter); robot_trajectory::RobotTrajectory -set_robot_trajectory_msg(const std::shared_ptr& robot_trajectory, - const moveit::core::RobotState& robot_state, const moveit_msgs::msg::RobotTrajectory& msg); +setRobotTrajectoryMsg(const std::shared_ptr& robot_trajectory, + const moveit::core::RobotState& robot_state, const moveit_msgs::msg::RobotTrajectory& msg); -void init_robot_trajectory(py::module& m); +void initRobotTrajectory(py::module& m); } // namespace bind_robot_trajectory } // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/transforms/transforms.cpp b/moveit_py/src/moveit/moveit_core/transforms/transforms.cpp index b2ab5408b3..fc79cdb5bc 100644 --- a/moveit_py/src/moveit/moveit_core/transforms/transforms.cpp +++ b/moveit_py/src/moveit/moveit_core/transforms/transforms.cpp @@ -40,13 +40,13 @@ namespace moveit_py { namespace bind_transforms { -Eigen::MatrixXd get_transform(std::shared_ptr& transforms, std::string& from_frame) +Eigen::MatrixXd getTransform(std::shared_ptr& transforms, std::string& from_frame) { auto transform = transforms->getTransform(from_frame); return transform.matrix(); } -std::map get_all_transforms(std::shared_ptr& transforms) +std::map getAllTransforms(std::shared_ptr& transforms) { std::map transforms_map; for (auto& transform : transforms->getAllTransforms()) @@ -56,7 +56,7 @@ std::map get_all_transforms(std::shared_ptr& transforms, std::string& from_frame); +Eigen::MatrixXd getTransform(std::shared_ptr& transforms, std::string& from_frame); -std::map get_all_transforms(std::shared_ptr& transforms); +std::map getAllTransforms(std::shared_ptr& transforms); -void init_transforms(py::module& m); +void initTransforms(py::module& m); } // namespace bind_transforms } // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.h b/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.h index cf36206f76..48c5540fe1 100644 --- a/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.h +++ b/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.h @@ -47,36 +47,36 @@ namespace moveit_py { namespace moveit_py_utils { -geometry_msgs::msg::PoseStamped PoseStampedToCpp(const py::object& pose_stamped); +geometry_msgs::msg::PoseStamped poseStampedToCpp(const py::object& pose_stamped); // TODO(peterdavidfagan): consider creating typecaster -geometry_msgs::msg::Pose PoseToCpp(const py::object& pose); -py::object PoseToPy(geometry_msgs::msg::Pose pose); +geometry_msgs::msg::Pose poseToCpp(const py::object& pose); +py::object poseToPy(geometry_msgs::msg::Pose pose); -geometry_msgs::msg::Point PointToCpp(const py::object& point); +geometry_msgs::msg::Point pointToCpp(const py::object& point); -geometry_msgs::msg::Vector3 Vector3ToCpp(const py::object& vector3); +geometry_msgs::msg::Vector3 vector3ToCpp(const py::object& vector3); -geometry_msgs::msg::Quaternion QuaternionToCpp(const py::object& quaternion); +geometry_msgs::msg::Quaternion quaternionToCpp(const py::object& quaternion); -shape_msgs::msg::SolidPrimitive SolidPrimitiveToCpp(const py::object& primitive); +shape_msgs::msg::SolidPrimitive solidPrimitiveToCpp(const py::object& primitive); -shape_msgs::msg::MeshTriangle MeshTriangleToCpp(const py::object& mesh_triangle); +shape_msgs::msg::MeshTriangle meshTriangleToCpp(const py::object& mesh_triangle); -shape_msgs::msg::Mesh MeshToCpp(const py::object& mesh); +shape_msgs::msg::Mesh meshToCpp(const py::object& mesh); -moveit_msgs::msg::BoundingVolume BoundingVolumeToCpp(const py::object& bounding_volume); +moveit_msgs::msg::BoundingVolume boundingVolumeToCpp(const py::object& bounding_volume); -moveit_msgs::msg::JointConstraint JointConstraintToCpp(const py::object& joint_constraint); +moveit_msgs::msg::JointConstraint jointConstraintToCpp(const py::object& joint_constraint); -moveit_msgs::msg::PositionConstraint PositionConstraintToCpp(const py::object& position_constraint); +moveit_msgs::msg::PositionConstraint positionConstraintToCpp(const py::object& position_constraint); -moveit_msgs::msg::OrientationConstraint OrientationConstraintToCpp(const py::object& orientation_constraint); +moveit_msgs::msg::OrientationConstraint orientationConstraintToCpp(const py::object& orientation_constraint); -moveit_msgs::msg::VisibilityConstraint VisibilityConstraintToCpp(const py::object& visibility_constraint); +moveit_msgs::msg::VisibilityConstraint visibilityConstraintToCpp(const py::object& visibility_constraint); -moveit_msgs::msg::CollisionObject CollisionObjectToCpp(const py::object& collision_object); +moveit_msgs::msg::CollisionObject collisionObjectToCpp(const py::object& collision_object); -moveit_msgs::msg::Constraints ConstraintsToCpp(const py::object& constraints); +moveit_msgs::msg::Constraints constraintsToCpp(const py::object& constraints); } // namespace moveit_py_utils } // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_py_utils/src/copy_ros_msg.cpp b/moveit_py/src/moveit/moveit_py_utils/src/copy_ros_msg.cpp index 20f222e769..407a979493 100644 --- a/moveit_py/src/moveit/moveit_py_utils/src/copy_ros_msg.cpp +++ b/moveit_py/src/moveit/moveit_py_utils/src/copy_ros_msg.cpp @@ -43,16 +43,16 @@ namespace moveit_py_utils { // Ros Message Copy Definitions (Note: copying faster than serialize/deserialize) -geometry_msgs::msg::PoseStamped PoseStampedToCpp(const py::object& pose_stamped) +geometry_msgs::msg::PoseStamped poseStampedToCpp(const py::object& pose_stamped) { // recreate instance in C++ using python object data geometry_msgs::msg::PoseStamped pose_stamped_cpp; pose_stamped_cpp.header.frame_id = pose_stamped.attr("header").attr("frame_id").cast(); - pose_stamped_cpp.pose = PoseToCpp(pose_stamped.attr("pose")); + pose_stamped_cpp.pose = poseToCpp(pose_stamped.attr("pose")); return pose_stamped_cpp; } -geometry_msgs::msg::Pose PoseToCpp(const py::object& pose) +geometry_msgs::msg::Pose poseToCpp(const py::object& pose) { // recreate instance in C++ using python object data geometry_msgs::msg::Pose pose_cpp; @@ -67,7 +67,7 @@ geometry_msgs::msg::Pose PoseToCpp(const py::object& pose) return pose_cpp; } -py::object PoseToPy(geometry_msgs::msg::Pose pose) +py::object poseToPy(geometry_msgs::msg::Pose pose) { // recreate instance in Python using C++ object data py::object pose_py = py::module_::import("geometry_msgs.msg").attr("Pose")(); @@ -83,7 +83,7 @@ py::object PoseToPy(geometry_msgs::msg::Pose pose) return pose_py; } -geometry_msgs::msg::Point PointToCpp(const py::object& point) +geometry_msgs::msg::Point pointToCpp(const py::object& point) { // recreate instance in C++ using python object data geometry_msgs::msg::Point point_cpp; @@ -94,7 +94,7 @@ geometry_msgs::msg::Point PointToCpp(const py::object& point) return point_cpp; } -geometry_msgs::msg::Vector3 Vector3ToCpp(const py::object& vector3) +geometry_msgs::msg::Vector3 vector3ToCpp(const py::object& vector3) { // recreate instance in C++ using python object data geometry_msgs::msg::Vector3 vector3_cpp; @@ -105,7 +105,7 @@ geometry_msgs::msg::Vector3 Vector3ToCpp(const py::object& vector3) return vector3_cpp; } -geometry_msgs::msg::Quaternion QuaternionToCpp(const py::object& quaternion) +geometry_msgs::msg::Quaternion quaternionToCpp(const py::object& quaternion) { // recreate instance in C++ using python object data geometry_msgs::msg::Quaternion quaternion_cpp; @@ -117,7 +117,7 @@ geometry_msgs::msg::Quaternion QuaternionToCpp(const py::object& quaternion) return quaternion_cpp; } -shape_msgs::msg::SolidPrimitive SolidPrimitiveToCpp(const py::object& primitive) +shape_msgs::msg::SolidPrimitive solidPrimitiveToCpp(const py::object& primitive) { // recreate instance in C++ using python object data shape_msgs::msg::SolidPrimitive primitive_cpp; @@ -130,7 +130,7 @@ shape_msgs::msg::SolidPrimitive SolidPrimitiveToCpp(const py::object& primitive) return primitive_cpp; } -shape_msgs::msg::MeshTriangle MeshTriangleToCpp(const py::object& mesh_triangle) +shape_msgs::msg::MeshTriangle meshTriangleToCpp(const py::object& mesh_triangle) { // recreate instance in C++ using python object data shape_msgs::msg::MeshTriangle mesh_triangle_cpp; @@ -141,25 +141,25 @@ shape_msgs::msg::MeshTriangle MeshTriangleToCpp(const py::object& mesh_triangle) return mesh_triangle_cpp; } -shape_msgs::msg::Mesh MeshToCpp(const py::object& mesh) +shape_msgs::msg::Mesh meshToCpp(const py::object& mesh) { // recreate instance in C++ using python object data shape_msgs::msg::Mesh mesh_cpp; mesh_cpp.vertices.resize(mesh.attr("vertices").attr("__len__")().cast()); for (const auto& vertex : mesh.attr("vertices")) { - mesh_cpp.vertices.push_back(PointToCpp(py::reinterpret_borrow(vertex))); + mesh_cpp.vertices.push_back(pointToCpp(py::reinterpret_borrow(vertex))); } mesh_cpp.triangles.resize(mesh.attr("triangles").attr("__len__")().cast()); for (const auto& triangle : mesh.attr("triangles")) { - mesh_cpp.triangles.push_back(MeshTriangleToCpp(py::reinterpret_borrow(triangle))); + mesh_cpp.triangles.push_back(meshTriangleToCpp(py::reinterpret_borrow(triangle))); } return mesh_cpp; } -moveit_msgs::msg::BoundingVolume BoundingVolumeToCpp(const py::object& bounding_volume) +moveit_msgs::msg::BoundingVolume boundingVolumeToCpp(const py::object& bounding_volume) { // recreate instance in C++ using python object data moveit_msgs::msg::BoundingVolume bounding_volume_cpp; @@ -167,31 +167,31 @@ moveit_msgs::msg::BoundingVolume BoundingVolumeToCpp(const py::object& bounding_ // primitives for (const auto& primitive : bounding_volume.attr("primitives")) { - bounding_volume_cpp.primitives.push_back(SolidPrimitiveToCpp(py::reinterpret_borrow(primitive))); + bounding_volume_cpp.primitives.push_back(solidPrimitiveToCpp(py::reinterpret_borrow(primitive))); } // primitive poses for (const auto& primitive_pose : bounding_volume.attr("primitive_poses")) { - bounding_volume_cpp.primitive_poses.push_back(PoseToCpp(py::reinterpret_borrow(primitive_pose))); + bounding_volume_cpp.primitive_poses.push_back(poseToCpp(py::reinterpret_borrow(primitive_pose))); } // meshes for (const auto& mesh : bounding_volume.attr("meshes")) { - bounding_volume_cpp.meshes.push_back(MeshToCpp(py::reinterpret_borrow(mesh))); + bounding_volume_cpp.meshes.push_back(meshToCpp(py::reinterpret_borrow(mesh))); } // mesh poses for (const auto& mesh_poses : bounding_volume.attr("mesh_poses")) { - bounding_volume_cpp.mesh_poses.push_back(PoseToCpp(py::reinterpret_borrow(mesh_poses))); + bounding_volume_cpp.mesh_poses.push_back(poseToCpp(py::reinterpret_borrow(mesh_poses))); } return bounding_volume_cpp; } -moveit_msgs::msg::JointConstraint JointConstraintToCpp(const py::object& joint_constraint) +moveit_msgs::msg::JointConstraint jointConstraintToCpp(const py::object& joint_constraint) { // recreate instance in C++ using python object data moveit_msgs::msg::JointConstraint joint_constraint_cpp; @@ -204,27 +204,27 @@ moveit_msgs::msg::JointConstraint JointConstraintToCpp(const py::object& joint_c return joint_constraint_cpp; } -moveit_msgs::msg::PositionConstraint PositionConstraintToCpp(const py::object& position_constraint) +moveit_msgs::msg::PositionConstraint positionConstraintToCpp(const py::object& position_constraint) { // recreate instance in C++ using python object data moveit_msgs::msg::PositionConstraint position_constraint_cpp; position_constraint_cpp.header.frame_id = position_constraint.attr("header").attr("frame_id").cast(); position_constraint_cpp.link_name = position_constraint.attr("link_name").cast(); - position_constraint_cpp.target_point_offset = Vector3ToCpp(position_constraint.attr("target_point_offset")); - position_constraint_cpp.constraint_region = BoundingVolumeToCpp(position_constraint.attr("constraint_region")); + position_constraint_cpp.target_point_offset = vector3ToCpp(position_constraint.attr("target_point_offset")); + position_constraint_cpp.constraint_region = boundingVolumeToCpp(position_constraint.attr("constraint_region")); position_constraint_cpp.weight = position_constraint.attr("weight").cast(); return position_constraint_cpp; } -moveit_msgs::msg::OrientationConstraint OrientationConstraintToCpp(const py::object& orientation_constraint) +moveit_msgs::msg::OrientationConstraint orientationConstraintToCpp(const py::object& orientation_constraint) { // recreate instance in C++ using python object data moveit_msgs::msg::OrientationConstraint orientation_constraint_cpp; orientation_constraint_cpp.header.frame_id = orientation_constraint.attr("header").attr("frame_id").cast(); orientation_constraint_cpp.link_name = orientation_constraint.attr("link_name").cast(); - orientation_constraint_cpp.orientation = QuaternionToCpp(orientation_constraint.attr("target_quaternion")); + orientation_constraint_cpp.orientation = quaternionToCpp(orientation_constraint.attr("target_quaternion")); orientation_constraint_cpp.absolute_x_axis_tolerance = orientation_constraint.attr("absolute_x_axis_tolerance").cast(); orientation_constraint_cpp.absolute_y_axis_tolerance = @@ -237,14 +237,14 @@ moveit_msgs::msg::OrientationConstraint OrientationConstraintToCpp(const py::obj return orientation_constraint_cpp; } -moveit_msgs::msg::VisibilityConstraint VisibilityConstraintToCpp(const py::object& visibility_constraint) +moveit_msgs::msg::VisibilityConstraint visibilityConstraintToCpp(const py::object& visibility_constraint) { // recreate instance in C++ using python object data moveit_msgs::msg::VisibilityConstraint visibility_constraint_cpp; visibility_constraint_cpp.target_radius = visibility_constraint.attr("target_radius").cast(); - visibility_constraint_cpp.target_pose = PoseStampedToCpp(visibility_constraint.attr("target_pose")); + visibility_constraint_cpp.target_pose = poseStampedToCpp(visibility_constraint.attr("target_pose")); visibility_constraint_cpp.cone_sides = visibility_constraint.attr("cone_sides").cast(); - visibility_constraint_cpp.sensor_pose = PoseStampedToCpp(visibility_constraint.attr("sensor_pose")); + visibility_constraint_cpp.sensor_pose = poseStampedToCpp(visibility_constraint.attr("sensor_pose")); visibility_constraint_cpp.max_view_angle = visibility_constraint.attr("max_view_angle").cast(); visibility_constraint_cpp.max_range_angle = visibility_constraint.attr("max_range_angle").cast(); visibility_constraint_cpp.sensor_view_direction = visibility_constraint.attr("sensor_view_direction").cast(); @@ -253,7 +253,7 @@ moveit_msgs::msg::VisibilityConstraint VisibilityConstraintToCpp(const py::objec return visibility_constraint_cpp; } -moveit_msgs::msg::CollisionObject CollisionObjectToCpp(const py::object& collision_object) +moveit_msgs::msg::CollisionObject collisionObjectToCpp(const py::object& collision_object) { // recreate instance in C++ using python object data moveit_msgs::msg::CollisionObject collision_object_cpp; @@ -262,7 +262,7 @@ moveit_msgs::msg::CollisionObject CollisionObjectToCpp(const py::object& collisi collision_object_cpp.header.frame_id = collision_object.attr("header").attr("frame_id").cast(); // object pose - collision_object_cpp.pose = PoseToCpp(collision_object.attr("pose")); + collision_object_cpp.pose = poseToCpp(collision_object.attr("pose")); // object id collision_object_cpp.id = collision_object.attr("id").cast(); @@ -274,14 +274,14 @@ moveit_msgs::msg::CollisionObject CollisionObjectToCpp(const py::object& collisi // iterate through python list creating C++ vector of primitives for (const auto& primitive : collision_object.attr("primitives")) { - auto primitive_cpp = SolidPrimitiveToCpp(py::reinterpret_borrow(primitive)); + auto primitive_cpp = solidPrimitiveToCpp(py::reinterpret_borrow(primitive)); collision_object_cpp.primitives.push_back(primitive_cpp); } // iterate through python list creating C++ vector of primitive poses for (const auto& primitive_pose : collision_object.attr("primitive_poses")) { - auto primitive_pose_cpp = PoseToCpp(py::reinterpret_borrow(primitive_pose)); + auto primitive_pose_cpp = poseToCpp(py::reinterpret_borrow(primitive_pose)); collision_object_cpp.primitive_poses.push_back(primitive_pose_cpp); } @@ -289,14 +289,14 @@ moveit_msgs::msg::CollisionObject CollisionObjectToCpp(const py::object& collisi for (const auto& mesh : collision_object.attr("meshes")) { // TODO (peterdavidfagan): implement mesh conversion - auto mesh_cpp = MeshToCpp(py::reinterpret_borrow(mesh)); + auto mesh_cpp = meshToCpp(py::reinterpret_borrow(mesh)); collision_object_cpp.meshes.push_back(mesh_cpp); } // iterate through python list creating C++ vector of mesh poses for (const auto& mesh_pose : collision_object.attr("mesh_poses")) { - auto mesh_pose_cpp = PoseToCpp(py::reinterpret_borrow(mesh_pose)); + auto mesh_pose_cpp = poseToCpp(py::reinterpret_borrow(mesh_pose)); collision_object_cpp.mesh_poses.push_back(mesh_pose_cpp); } @@ -306,7 +306,7 @@ moveit_msgs::msg::CollisionObject CollisionObjectToCpp(const py::object& collisi return collision_object_cpp; } -moveit_msgs::msg::Constraints ConstraintsToCpp(const py::object& constraints) +moveit_msgs::msg::Constraints constraintsToCpp(const py::object& constraints) { // recreate instance in C++ using python object data moveit_msgs::msg::Constraints constraints_cpp; @@ -314,14 +314,14 @@ moveit_msgs::msg::Constraints ConstraintsToCpp(const py::object& constraints) // iterate through python list creating C++ vector of joint constraints for (const auto& joint_constraint : constraints.attr("joint_constraints")) { - auto joint_constraint_cpp = JointConstraintToCpp(py::reinterpret_borrow(joint_constraint)); + auto joint_constraint_cpp = jointConstraintToCpp(py::reinterpret_borrow(joint_constraint)); constraints_cpp.joint_constraints.push_back(joint_constraint_cpp); } // iterate through python list creating C++ vector of position constraints for (const auto& position_constraint : constraints.attr("position_constraints")) { - auto position_constraint_cpp = PositionConstraintToCpp(py::reinterpret_borrow(position_constraint)); + auto position_constraint_cpp = positionConstraintToCpp(py::reinterpret_borrow(position_constraint)); constraints_cpp.position_constraints.push_back(position_constraint_cpp); } @@ -329,7 +329,7 @@ moveit_msgs::msg::Constraints ConstraintsToCpp(const py::object& constraints) for (const auto& orientation_constraint : constraints.attr("orientation_constraints")) { auto orientation_constraint_cpp = - OrientationConstraintToCpp(py::reinterpret_borrow(orientation_constraint)); + orientationConstraintToCpp(py::reinterpret_borrow(orientation_constraint)); constraints_cpp.orientation_constraints.push_back(orientation_constraint_cpp); } @@ -337,7 +337,7 @@ moveit_msgs::msg::Constraints ConstraintsToCpp(const py::object& constraints) for (const auto& visibility_constraint : constraints.attr("visibility_constraints")) { auto visibility_constraint_cpp = - VisibilityConstraintToCpp(py::reinterpret_borrow(visibility_constraint)); + visibilityConstraintToCpp(py::reinterpret_borrow(visibility_constraint)); constraints_cpp.visibility_constraints.push_back(visibility_constraint_cpp); } diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp index ed43b05e24..fe7ff81098 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp @@ -41,12 +41,12 @@ namespace moveit_py namespace bind_moveit_cpp { std::shared_ptr -get_planning_component(std::shared_ptr& moveit_cpp_ptr, const std::string& planning_component) +getPlanningComponent(std::shared_ptr& moveit_cpp_ptr, const std::string& planning_component) { return std::make_shared(planning_component, moveit_cpp_ptr); } -void init_moveit_py(py::module& m) +void initMoveitPy(py::module& m) { auto utils = py::module::import("moveit.utils"); @@ -144,7 +144,7 @@ void init_moveit_py(py::module& m) R"( Execute a trajectory (planning group is inferred from robot trajectory object). )") - .def("get_planning_component", &moveit_py::bind_moveit_cpp::get_planning_component, + .def("get_planning_component", &moveit_py::bind_moveit_cpp::getPlanningComponent, py::arg("planning_component_name"), py::return_value_policy::take_ownership, R"( Creates a planning component instance. diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.h b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.h index 4e95652267..6c287ebe39 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.h +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.h @@ -55,6 +55,6 @@ namespace moveit_py { namespace bind_moveit_cpp { -void init_moveit_py(py::module& m); +void initMoveitPy(py::module& m); } // namespace bind_moveit_cpp } // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp index 3e6488ca31..c21e9b104e 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp @@ -104,10 +104,10 @@ plan(std::shared_ptr& planning_component, } } -bool set_goal(std::shared_ptr& planning_component, - std::optional configuration_name, std::optional robot_state, - std::optional pose_stamped_msg, std::optional pose_link, - std::optional> motion_plan_constraints) +bool setGoal(std::shared_ptr& planning_component, + std::optional configuration_name, std::optional robot_state, + std::optional pose_stamped_msg, std::optional pose_link, + std::optional> motion_plan_constraints) { // check that no more than one argument is specified if (configuration_name && robot_state) @@ -167,8 +167,8 @@ bool set_goal(std::shared_ptr& planning_component } } -bool set_start_state(std::shared_ptr& planning_component, - std::optional configuration_name, std::optional robot_state) +bool setStartState(std::shared_ptr& planning_component, + std::optional configuration_name, std::optional robot_state) { // check that no more than one argument is specified if (configuration_name && robot_state) @@ -194,7 +194,7 @@ bool set_start_state(std::shared_ptr& planning_co } } -void init_plan_request_parameters(py::module& m) +void initPlanRequestParameters(py::module& m) { py::class_>(m, "PlanRequestParameters", @@ -235,7 +235,7 @@ void init_plan_request_parameters(py::module& m) )"); } -void init_multi_plan_request_parameters(py::module& m) +void initMultiPlanRequestParameters(py::module& m) { py::class_>( @@ -252,7 +252,7 @@ void init_multi_plan_request_parameters(py::module& m) .def_readonly("multi_plan_request_parameters", &moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters::plan_request_parameter_vector); } -void init_planning_component(py::module& m) +void initPlanningComponent(py::module& m) { py::class_>(m, "PlanningComponent", R"( @@ -290,7 +290,7 @@ void init_planning_component(py::module& m) Set the start state of the plan to the current state of the robot. )") - .def("set_start_state", &moveit_py::bind_planning_component::set_start_state, + .def("set_goal_state", &moveit_py::bind_planning_component::setStartState, py::arg("configuration_name") = nullptr, py::arg("robot_state") = nullptr, R"( Set the start state of the plan to the given robot state. @@ -311,7 +311,7 @@ void init_planning_component(py::module& m) py::overload_cast&, std::optional, std::optional, std::optional, std::optional, std::optional>>( - &moveit_py::bind_planning_component::set_goal), + &moveit_py::bind_planning_component::setGoal), py::arg("configuration_name") = nullptr, py::arg("robot_state") = nullptr, py::arg("pose_stamped_msg") = nullptr, py::arg("pose_link") = nullptr, py::arg("motion_plan_constraints") = nullptr, diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.h b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.h index fd727b9d7c..f0a7c04cee 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.h +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.h @@ -66,19 +66,18 @@ plan(std::shared_ptr& planning_component, std::optional solution_selection_function, std::optional stopping_criterion_callback); -bool set_goal(std::shared_ptr& planning_component, - std::optional configuration_name, std::optional robot_state, - std::optional pose_stamped_msg, std::optional pose_link, - std::optional> motion_plan_constraints); +bool setGoal(std::shared_ptr& planning_component, + std::optional configuration_name, std::optional robot_state, + std::optional pose_stamped_msg, std::optional pose_link, + std::optional> motion_plan_constraints); -bool set_start_state(std::shared_ptr& planning_component, - std::optional configuration_name, - std::optional robot_state); +bool setStartState(std::shared_ptr& planning_component, + std::optional configuration_name, std::optional robot_state); -void init_plan_request_parameters(py::module& m); +void initPlanRequestParameters(py::module& m); -void init_multi_plan_request_parameters(py::module& m); +void initMultiPlanRequestParameters(py::module& m); -void init_planning_component(py::module& m); +void initPlanningComponent(py::module& m); } // namespace bind_planning_component } // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp index c18e57732a..f59103e3cb 100644 --- a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp +++ b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp @@ -43,13 +43,13 @@ namespace bind_planning_scene_monitor static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_py.bind_planning_scene_monitor"); LockedPlanningSceneContextManagerRO -read_only(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) +readOnly(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) { return LockedPlanningSceneContextManagerRO(planning_scene_monitor); }; LockedPlanningSceneContextManagerRW -read_write(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) +readWrite(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) { return LockedPlanningSceneContextManagerRW(planning_scene_monitor); }; @@ -80,8 +80,8 @@ void LockedPlanningSceneContextManagerRW::lockedPlanningSceneRwExit(const py::ob } // TODO: simplify with typecaster -void apply_planning_scene(std::shared_ptr& planning_scene_monitor, - const moveit_msgs::msg::PlanningScene& planning_scene) +void applyPlanningScene(std::shared_ptr& planning_scene_monitor, + const moveit_msgs::msg::PlanningScene& planning_scene) { // lock planning scene { @@ -90,7 +90,7 @@ void apply_planning_scene(std::shared_ptr( m, "PlanningSceneMonitor", R"( @@ -132,18 +132,18 @@ void init_planning_scene_monitor(py::module& m) Clears the octomap. )") - .def("read_only", &moveit_py::bind_planning_scene_monitor::read_only, + .def("read_only", &moveit_py::bind_planning_scene_monitor::readOnly, R"( Returns a read-only context manager for the planning scene. )") - .def("read_write", &moveit_py::bind_planning_scene_monitor::read_write, + .def("read_write", &moveit_py::bind_planning_scene_monitor::readWrite, R"( Returns a read-write context manager for the planning scene. )"); } -void init_context_managers(py::module& m) +void initContextManagers(py::module& m) { // In Python we lock the planning scene using a with statement as this allows us to have control over resources. // To this end each of the below manager classes binds special methods __enter__ and __exit__. diff --git a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.h b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.h index 15f6a29d1b..e6b5e514bf 100644 --- a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.h +++ b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.h @@ -85,21 +85,21 @@ class LockedPlanningSceneContextManagerRO }; LockedPlanningSceneContextManagerRW -read_write(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); +readWrite(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); LockedPlanningSceneContextManagerRO -read_only(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); +readOnly(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); // const planning_scene::PlanningScenePtr& locked_planning_scene_enter_(LockedPlanningSceneContextManager& context_manager); // void locked_planning_scene_exit_(LockedPlanningSceneContextManager& context_manager, const py::object& type, // const py::object& value, const py::object& traceback); -void apply_planning_scene(std::shared_ptr& planning_scene_monitor, - const moveit_msgs::msg::PlanningScene& planning_scene); +void applyPlanningScene(std::shared_ptr& planning_scene_monitor, + const moveit_msgs::msg::PlanningScene& planning_scene); -void init_planning_scene_monitor(py::module& m); +void initPlanningSceneMonitor(py::module& m); -void init_context_managers(py::module& m); +void initContextManagers(py::module& m); } // namespace bind_planning_scene_monitor } // namespace moveit_py diff --git a/moveit_py/src/moveit/planning.cpp b/moveit_py/src/moveit/planning.cpp index 93e21b1624..f89ffc50e9 100644 --- a/moveit_py/src/moveit/planning.cpp +++ b/moveit_py/src/moveit/planning.cpp @@ -47,10 +47,10 @@ PYBIND11_MODULE(planning, m) options.disable_function_signatures(); // Construct module classes - moveit_py::bind_planning_component::init_plan_request_parameters(m); - moveit_py::bind_planning_component::init_multi_plan_request_parameters(m); - moveit_py::bind_planning_component::init_planning_component(m); - moveit_py::bind_planning_scene_monitor::init_planning_scene_monitor(m); - moveit_py::bind_planning_scene_monitor::init_context_managers(m); - moveit_py::bind_moveit_cpp::init_moveit_py(m); + moveit_py::bind_planning_component::initPlanRequestParameters(m); + moveit_py::bind_planning_component::initMultiPlanRequestParameters(m); + moveit_py::bind_planning_component::initPlanningComponent(m); + moveit_py::bind_planning_scene_monitor::initPlanningSceneMonitor(m); + moveit_py::bind_planning_scene_monitor::initContextManagers(m); + moveit_py::bind_moveit_cpp::initMoveitPy(m); } 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/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp b/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp index a70ebe4685..5b987e9f8c 100644 --- a/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp @@ -109,7 +109,7 @@ bool GlobalPlannerComponent::initializeGlobalPlanner() return rclcpp_action::CancelResponse::ACCEPT; }, // Execution callback - [this](std::shared_ptr> goal_handle) { + [this](const std::shared_ptr>& goal_handle) { // this needs to return quickly to avoid blocking the executor, so spin up a new thread if (long_callback_thread_.joinable()) { diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp index b1584123ad..b9a1426934 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp @@ -161,7 +161,7 @@ bool HybridPlanningManager::initialize() return rclcpp_action::CancelResponse::ACCEPT; }, // Execution callback - [this](std::shared_ptr> goal_handle) { + [this](const std::shared_ptr>& goal_handle) { // this needs to return quickly to avoid blocking the executor, so spin up a new thread if (long_callback_thread_.joinable()) { 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/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h index b9a7a04415..d41e6f2a50 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h @@ -609,7 +609,7 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor * a member of this class. However because of the "operator->" here * which returns a PlanningSceneConstPtr, this works. * - * Any number of these "ReadOnly" locks can exist at a given time. + * Any number of these "read_only" locks can exist at a given time. * The intention is that users which only need to read from the * PlanningScene will use these and will thus not interfere with each * other. 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); } diff --git a/moveit_ros/warehouse/src/broadcast.cpp b/moveit_ros/warehouse/src/broadcast.cpp index 7a5cd4aafa..5124fa000e 100644 --- a/moveit_ros/warehouse/src/broadcast.cpp +++ b/moveit_ros/warehouse/src/broadcast.cpp @@ -66,7 +66,7 @@ static const std::string CONSTRAINTS_TOPIC = "constraints"; static const std::string STATES_TOPIC = "robot_states"; using namespace std::chrono_literals; -using moveit::get_logger; +using moveit::getLogger; int main(int argc, char** argv) { @@ -75,7 +75,7 @@ int main(int argc, char** argv) node_options.allow_undeclared_parameters(true); node_options.automatically_declare_parameters_from_overrides(true); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("publish_warehouse_data", node_options); - moveit::get_logger_mut() = node->get_logger(); + moveit::getLoggerMut() = node->get_logger(); // time to wait in between publishing messages double delay = 0.001; @@ -142,7 +142,7 @@ int main(int argc, char** argv) moveit_warehouse::PlanningSceneWithMetadata pswm; if (pss.getPlanningScene(pswm, scene_name)) { - RCLCPP_INFO(get_logger(), "Publishing scene '%s'", + RCLCPP_INFO(getLogger(), "Publishing scene '%s'", pswm->lookupString(moveit_warehouse::PlanningSceneStorage::PLANNING_SCENE_ID_NAME).c_str()); pub_scene->publish(static_cast(*pswm)); executor.spin_once(0ns); @@ -155,14 +155,14 @@ int main(int argc, char** argv) std::vector planning_queries; std::vector query_names; pss.getPlanningQueries(planning_queries, query_names, pswm->name); - RCLCPP_INFO(get_logger(), "There are %d planning queries associated to the scene", + RCLCPP_INFO(getLogger(), "There are %d planning queries associated to the scene", static_cast(planning_queries.size())); rclcpp::sleep_for(500ms); for (std::size_t i = 0; i < planning_queries.size(); ++i) { if (req) { - RCLCPP_INFO(get_logger(), "Publishing query '%s'", query_names[i].c_str()); + RCLCPP_INFO(getLogger(), "Publishing query '%s'", query_names[i].c_str()); pub_req->publish(static_cast(*planning_queries[i])); executor.spin_once(0ns); } @@ -196,7 +196,7 @@ int main(int argc, char** argv) moveit_warehouse::ConstraintsWithMetadata cwm; if (cs.getConstraints(cwm, cname)) { - RCLCPP_INFO(get_logger(), "Publishing constraints '%s'", + RCLCPP_INFO(getLogger(), "Publishing constraints '%s'", cwm->lookupString(moveit_warehouse::ConstraintsStorage::CONSTRAINTS_ID_NAME).c_str()); pub_constr->publish(static_cast(*cwm)); executor.spin_once(0ns); @@ -218,7 +218,7 @@ int main(int argc, char** argv) moveit_warehouse::RobotStateWithMetadata rswm; if (rs.getRobotState(rswm, rname)) { - RCLCPP_INFO(get_logger(), "Publishing state '%s'", + RCLCPP_INFO(getLogger(), "Publishing state '%s'", rswm->lookupString(moveit_warehouse::RobotStateStorage::STATE_NAME).c_str()); pub_state->publish(static_cast(*rswm)); executor.spin_once(0ns); @@ -228,7 +228,7 @@ int main(int argc, char** argv) } rclcpp::sleep_for(1s); - RCLCPP_INFO(get_logger(), "Done."); + RCLCPP_INFO(getLogger(), "Done."); return 0; } diff --git a/moveit_ros/warehouse/src/constraints_storage.cpp b/moveit_ros/warehouse/src/constraints_storage.cpp index ead85bb713..ca56e58d6e 100644 --- a/moveit_ros/warehouse/src/constraints_storage.cpp +++ b/moveit_ros/warehouse/src/constraints_storage.cpp @@ -49,7 +49,7 @@ using warehouse_ros::Metadata; using warehouse_ros::Query; moveit_warehouse::ConstraintsStorage::ConstraintsStorage(warehouse_ros::DatabaseConnection::Ptr conn) - : MoveItMessageStorage(std::move(conn)), logger_(moveit::make_child_logger("moveit_warehouse_constraints_storage")) + : MoveItMessageStorage(std::move(conn)), logger_(moveit::makeChildLogger("moveit_warehouse_constraints_storage")) { createCollections(); } diff --git a/moveit_ros/warehouse/src/import_from_text.cpp b/moveit_ros/warehouse/src/import_from_text.cpp index 4370a106f9..c345d033aa 100644 --- a/moveit_ros/warehouse/src/import_from_text.cpp +++ b/moveit_ros/warehouse/src/import_from_text.cpp @@ -49,7 +49,7 @@ static const std::string ROBOT_DESCRIPTION = "robot_description"; -using moveit::get_logger; +using moveit::getLogger; void parseStart(std::istream& in, planning_scene_monitor::PlanningSceneMonitor* psm, moveit_warehouse::RobotStateStorage* rs) @@ -91,7 +91,7 @@ void parseStart(std::istream& in, planning_scene_monitor::PlanningSceneMonitor* st.setVariablePositions(v); moveit_msgs::msg::RobotState msg; moveit::core::robotStateToRobotStateMsg(st, msg); - RCLCPP_INFO(get_logger(), "Parsed start state '%s'", name.c_str()); + RCLCPP_INFO(getLogger(), "Parsed start state '%s'", name.c_str()); rs->addRobotState(msg, name); } } @@ -136,7 +136,7 @@ void parseLinkConstraint(std::istream& in, planning_scene_monitor::PlanningScene Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ())); } else - RCLCPP_ERROR(get_logger(), "Unknown link constraint element: '%s'", type.c_str()); + RCLCPP_ERROR(getLogger(), "Unknown link constraint element: '%s'", type.c_str()); in >> type; } @@ -151,7 +151,7 @@ void parseLinkConstraint(std::istream& in, planning_scene_monitor::PlanningScene pose.header.frame_id = psm->getRobotModel()->getModelFrame(); moveit_msgs::msg::Constraints constr = kinematic_constraints::constructGoalConstraints(link_name, pose); constr.name = name; - RCLCPP_INFO(get_logger(), "Parsed link constraint '%s'", name.c_str()); + RCLCPP_INFO(getLogger(), "Parsed link constraint '%s'", name.c_str()); cs->addConstraints(constr); } } @@ -181,7 +181,7 @@ void parseGoal(std::istream& in, planning_scene_monitor::PlanningSceneMonitor* p } else { - RCLCPP_INFO(get_logger(), "Unknown goal type: '%s'", type.c_str()); + RCLCPP_INFO(getLogger(), "Unknown goal type: '%s'", type.c_str()); } } } @@ -210,7 +210,7 @@ void parseQueries(std::istream& in, planning_scene_monitor::PlanningSceneMonitor } else { - RCLCPP_ERROR(get_logger(), "Unknown query type: '%s'", type.c_str()); + RCLCPP_ERROR(getLogger(), "Unknown query type: '%s'", type.c_str()); } } } @@ -223,7 +223,7 @@ int main(int argc, char** argv) node_options.allow_undeclared_parameters(true); node_options.automatically_declare_parameters_from_overrides(true); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("import_from_text_to_warehouse", node_options); - moveit::get_logger_mut() = node->get_logger(); + moveit::getLoggerMut() = node->get_logger(); // clang-format off boost::program_options::options_description desc; @@ -253,7 +253,7 @@ int main(int argc, char** argv) planning_scene_monitor::PlanningSceneMonitor psm(node, ROBOT_DESCRIPTION); if (!psm.getPlanningScene()) { - RCLCPP_ERROR(get_logger(), "Unable to initialize PlanningSceneMonitor"); + RCLCPP_ERROR(getLogger(), "Unable to initialize PlanningSceneMonitor"); return 1; } diff --git a/moveit_ros/warehouse/src/initialize_demo_db.cpp b/moveit_ros/warehouse/src/initialize_demo_db.cpp index b1350890c4..4e93225909 100644 --- a/moveit_ros/warehouse/src/initialize_demo_db.cpp +++ b/moveit_ros/warehouse/src/initialize_demo_db.cpp @@ -56,7 +56,7 @@ static const std::string ROBOT_DESCRIPTION = "robot_description"; -using moveit::get_logger; +using moveit::getLogger; int main(int argc, char** argv) { @@ -65,7 +65,7 @@ int main(int argc, char** argv) node_options.allow_undeclared_parameters(true); node_options.automatically_declare_parameters_from_overrides(true); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("initialize_demo_db", node_options); - moveit::get_logger_mut() = node->get_logger(); + moveit::getLoggerMut() = node->get_logger(); boost::program_options::options_description desc; desc.add_options()("help", "Show help message")("host", boost::program_options::value(), @@ -92,7 +92,7 @@ int main(int argc, char** argv) planning_scene_monitor::PlanningSceneMonitor psm(node, ROBOT_DESCRIPTION); if (!psm.getPlanningScene()) { - RCLCPP_ERROR(get_logger(), "Unable to initialize PlanningSceneMonitor"); + RCLCPP_ERROR(getLogger(), "Unable to initialize PlanningSceneMonitor"); return 1; } @@ -108,12 +108,12 @@ int main(int argc, char** argv) psm.getPlanningScene()->getPlanningSceneMsg(psmsg); psmsg.name = "default"; pss.addPlanningScene(psmsg); - RCLCPP_INFO(get_logger(), "Added default scene: '%s'", psmsg.name.c_str()); + RCLCPP_INFO(getLogger(), "Added default scene: '%s'", psmsg.name.c_str()); moveit_msgs::msg::RobotState rsmsg; moveit::core::robotStateToRobotStateMsg(psm.getPlanningScene()->getCurrentState(), rsmsg); rs.addRobotState(rsmsg, "default"); - RCLCPP_INFO(get_logger(), "Added default state"); + RCLCPP_INFO(getLogger(), "Added default state"); const std::vector& gnames = psm.getRobotModel()->getJointModelGroupNames(); for (const std::string& gname : gnames) @@ -140,9 +140,9 @@ int main(int argc, char** argv) cmsg.orientation_constraints.resize(1, ocm); cmsg.name = ocm.link_name + ":upright"; cs.addConstraints(cmsg, psm.getRobotModel()->getName(), jmg->getName()); - RCLCPP_INFO(get_logger(), "Added default constraint: '%s'", cmsg.name.c_str()); + RCLCPP_INFO(getLogger(), "Added default constraint: '%s'", cmsg.name.c_str()); } - RCLCPP_INFO(get_logger(), "Default MoveIt Warehouse was reset."); + RCLCPP_INFO(getLogger(), "Default MoveIt Warehouse was reset."); rclcpp::spin(node); diff --git a/moveit_ros/warehouse/src/planning_scene_storage.cpp b/moveit_ros/warehouse/src/planning_scene_storage.cpp index 439c6c1c21..dd5bbd1996 100644 --- a/moveit_ros/warehouse/src/planning_scene_storage.cpp +++ b/moveit_ros/warehouse/src/planning_scene_storage.cpp @@ -49,7 +49,7 @@ using warehouse_ros::Metadata; using warehouse_ros::Query; moveit_warehouse::PlanningSceneStorage::PlanningSceneStorage(warehouse_ros::DatabaseConnection::Ptr conn) - : MoveItMessageStorage(std::move(conn)), logger_(moveit::make_child_logger("moveit_warehouse_planning_scene_storage")) + : MoveItMessageStorage(std::move(conn)), logger_(moveit::makeChildLogger("moveit_warehouse_planning_scene_storage")) { createCollections(); } diff --git a/moveit_ros/warehouse/src/planning_scene_world_storage.cpp b/moveit_ros/warehouse/src/planning_scene_world_storage.cpp index 4a4e631302..066044b135 100644 --- a/moveit_ros/warehouse/src/planning_scene_world_storage.cpp +++ b/moveit_ros/warehouse/src/planning_scene_world_storage.cpp @@ -47,7 +47,7 @@ using warehouse_ros::Query; moveit_warehouse::PlanningSceneWorldStorage::PlanningSceneWorldStorage(warehouse_ros::DatabaseConnection::Ptr conn) : MoveItMessageStorage(std::move(conn)) - , logger_(moveit::make_child_logger("moveit_warehouse_planning_scene_world_storage")) + , logger_(moveit::makeChildLogger("moveit_warehouse_planning_scene_world_storage")) { createCollections(); } diff --git a/moveit_ros/warehouse/src/save_as_text.cpp b/moveit_ros/warehouse/src/save_as_text.cpp index 187d2d5ab0..96e9780db3 100644 --- a/moveit_ros/warehouse/src/save_as_text.cpp +++ b/moveit_ros/warehouse/src/save_as_text.cpp @@ -56,7 +56,7 @@ static const std::string ROBOT_DESCRIPTION = "robot_description"; typedef std::pair LinkConstraintPair; typedef std::map LinkConstraintMap; -using moveit::get_logger; +using moveit::getLogger; void collectLinkConstraints(const moveit_msgs::msg::Constraints& constraints, LinkConstraintMap& lcmap) { @@ -76,7 +76,7 @@ void collectLinkConstraints(const moveit_msgs::msg::Constraints& constraints, Li } else { - RCLCPP_WARN(get_logger(), "Orientation constraint for %s has no matching position constraint", + RCLCPP_WARN(getLogger(), "Orientation constraint for %s has no matching position constraint", orientation_constraint.link_name.c_str()); } } @@ -89,7 +89,7 @@ int main(int argc, char** argv) node_options.allow_undeclared_parameters(true); node_options.automatically_declare_parameters_from_overrides(true); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("save_warehouse_as_text", node_options); - moveit::get_logger_mut() = node->get_logger(); + moveit::getLoggerMut() = node->get_logger(); boost::program_options::options_description desc; desc.add_options()("help", "Show help message")("host", boost::program_options::value(), @@ -127,7 +127,7 @@ int main(int argc, char** argv) moveit_warehouse::PlanningSceneWithMetadata pswm; if (pss.getPlanningScene(pswm, scene_name)) { - RCLCPP_INFO(get_logger(), "Saving scene '%s'", scene_name.c_str()); + RCLCPP_INFO(getLogger(), "Saving scene '%s'", scene_name.c_str()); psm.getPlanningScene()->setPlanningSceneMsg(static_cast(*pswm)); std::ofstream fout((scene_name + ".scene").c_str()); psm.getPlanningScene()->saveGeometryToStream(fout); @@ -157,8 +157,7 @@ int main(int argc, char** argv) qfout << robot_state_names.size() << '\n'; for (const std::string& robot_state_name : robot_state_names) { - RCLCPP_INFO(get_logger(), "Saving start state %s for scene %s", robot_state_name.c_str(), - scene_name.c_str()); + RCLCPP_INFO(getLogger(), "Saving start state %s for scene %s", robot_state_name.c_str(), scene_name.c_str()); qfout << robot_state_name << '\n'; moveit_warehouse::RobotStateWithMetadata robot_state; rss.getRobotState(robot_state, robot_state_name); @@ -175,7 +174,7 @@ int main(int argc, char** argv) qfout << constraint_names.size() << '\n'; for (const std::string& constraint_name : constraint_names) { - RCLCPP_INFO(get_logger(), "Saving goal %s for scene %s", constraint_name.c_str(), scene_name.c_str()); + RCLCPP_INFO(getLogger(), "Saving goal %s for scene %s", constraint_name.c_str(), scene_name.c_str()); qfout << "link_constraint" << '\n'; qfout << constraint_name << '\n'; moveit_warehouse::ConstraintsWithMetadata constraints; @@ -201,7 +200,7 @@ int main(int argc, char** argv) } } - RCLCPP_INFO(get_logger(), "Done."); + RCLCPP_INFO(getLogger(), "Done."); rclcpp::spin(node); return 0; } diff --git a/moveit_ros/warehouse/src/save_to_warehouse.cpp b/moveit_ros/warehouse/src/save_to_warehouse.cpp index ffff2f9bdd..f4d68a10ad 100644 --- a/moveit_ros/warehouse/src/save_to_warehouse.cpp +++ b/moveit_ros/warehouse/src/save_to_warehouse.cpp @@ -60,13 +60,13 @@ #include #include -using moveit::get_logger; +using moveit::getLogger; static const std::string ROBOT_DESCRIPTION = "robot_description"; void onSceneUpdate(planning_scene_monitor::PlanningSceneMonitor& psm, moveit_warehouse::PlanningSceneStorage& pss) { - RCLCPP_INFO(get_logger(), "Received an update to the planning scene..."); + RCLCPP_INFO(getLogger(), "Received an update to the planning scene..."); if (!psm.getPlanningScene()->getName().empty()) { @@ -78,12 +78,12 @@ void onSceneUpdate(planning_scene_monitor::PlanningSceneMonitor& psm, moveit_war } else { - RCLCPP_INFO(get_logger(), "Scene '%s' was previously added. Not adding again.", + RCLCPP_INFO(getLogger(), "Scene '%s' was previously added. Not adding again.", psm.getPlanningScene()->getName().c_str()); } } else - RCLCPP_INFO(get_logger(), "Scene name is empty. Not saving."); + RCLCPP_INFO(getLogger(), "Scene name is empty. Not saving."); } void onMotionPlanRequest(const moveit_msgs::msg::MotionPlanRequest& req, @@ -91,7 +91,7 @@ void onMotionPlanRequest(const moveit_msgs::msg::MotionPlanRequest& req, { if (psm.getPlanningScene()->getName().empty()) { - RCLCPP_INFO(get_logger(), "Scene name is empty. Not saving planning request."); + RCLCPP_INFO(getLogger(), "Scene name is empty. Not saving planning request."); return; } pss.addPlanningQuery(req, psm.getPlanningScene()->getName()); @@ -101,19 +101,19 @@ void onConstraints(const moveit_msgs::msg::Constraints& msg, moveit_warehouse::C { if (msg.name.empty()) { - RCLCPP_INFO(get_logger(), "No name specified for constraints. Not saving."); + RCLCPP_INFO(getLogger(), "No name specified for constraints. Not saving."); return; } if (cs.hasConstraints(msg.name)) { - RCLCPP_INFO(get_logger(), "Replacing constraints '%s'", msg.name.c_str()); + RCLCPP_INFO(getLogger(), "Replacing constraints '%s'", msg.name.c_str()); cs.removeConstraints(msg.name); cs.addConstraints(msg); } else { - RCLCPP_INFO(get_logger(), "Adding constraints '%s'", msg.name.c_str()); + RCLCPP_INFO(getLogger(), "Adding constraints '%s'", msg.name.c_str()); cs.addConstraints(msg); } } @@ -127,7 +127,7 @@ void onRobotState(const moveit_msgs::msg::RobotState& msg, moveit_warehouse::Rob while (names_set.find("S" + std::to_string(n)) != names_set.end()) n++; std::string name = "S" + std::to_string(n); - RCLCPP_INFO(get_logger(), "Adding robot state '%s'", name.c_str()); + RCLCPP_INFO(getLogger(), "Adding robot state '%s'", name.c_str()); rs.addRobotState(msg, name); } @@ -138,7 +138,7 @@ int main(int argc, char** argv) node_options.allow_undeclared_parameters(true); node_options.automatically_declare_parameters_from_overrides(true); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("save_to_warehouse", node_options); - moveit::get_logger_mut() = node->get_logger(); + moveit::getLoggerMut() = node->get_logger(); boost::program_options::options_description desc; desc.add_options()("help", "Show help message")("host", boost::program_options::value(), @@ -165,7 +165,7 @@ int main(int argc, char** argv) planning_scene_monitor::PlanningSceneMonitor psm(node, ROBOT_DESCRIPTION); if (!psm.getPlanningScene()) { - RCLCPP_ERROR(get_logger(), "Unable to initialize PlanningSceneMonitor"); + RCLCPP_ERROR(getLogger(), "Unable to initialize PlanningSceneMonitor"); return 1; } @@ -178,35 +178,35 @@ int main(int argc, char** argv) pss.getPlanningSceneNames(names); if (names.empty()) { - RCLCPP_INFO(get_logger(), "There are no previously stored scenes"); + RCLCPP_INFO(getLogger(), "There are no previously stored scenes"); } else { - RCLCPP_INFO(get_logger(), "Previously stored scenes:"); + RCLCPP_INFO(getLogger(), "Previously stored scenes:"); for (const std::string& name : names) - RCLCPP_INFO(get_logger(), " * %s", name.c_str()); + RCLCPP_INFO(getLogger(), " * %s", name.c_str()); } cs.getKnownConstraints(names); if (names.empty()) { - RCLCPP_INFO(get_logger(), "There are no previously stored constraints"); + RCLCPP_INFO(getLogger(), "There are no previously stored constraints"); } else { - RCLCPP_INFO(get_logger(), "Previously stored constraints:"); + RCLCPP_INFO(getLogger(), "Previously stored constraints:"); for (const std::string& name : names) - RCLCPP_INFO(get_logger(), " * %s", name.c_str()); + RCLCPP_INFO(getLogger(), " * %s", name.c_str()); } rs.getKnownRobotStates(names); if (names.empty()) { - RCLCPP_INFO(get_logger(), "There are no previously stored robot states"); + RCLCPP_INFO(getLogger(), "There are no previously stored robot states"); } else { - RCLCPP_INFO(get_logger(), "Previously stored robot states:"); + RCLCPP_INFO(getLogger(), "Previously stored robot states:"); for (const std::string& name : names) - RCLCPP_INFO(get_logger(), " * %s", name.c_str()); + RCLCPP_INFO(getLogger(), " * %s", name.c_str()); } psm.addUpdateCallback([&](auto&&) { return onSceneUpdate(psm, pss); }); @@ -223,11 +223,11 @@ int main(int argc, char** argv) std::vector topics; psm.getMonitoredTopics(topics); - RCLCPP_INFO_STREAM(get_logger(), + RCLCPP_INFO_STREAM(getLogger(), "Listening for scene updates on topics " << fmt::format("{}", fmt::join(topics, ", "))); - RCLCPP_INFO_STREAM(get_logger(), "Listening for planning requests on topic " << mplan_req_sub->get_topic_name()); - RCLCPP_INFO_STREAM(get_logger(), "Listening for named constraints on topic " << constr_sub->get_topic_name()); - RCLCPP_INFO_STREAM(get_logger(), "Listening for states on topic " << state_sub->get_topic_name()); + RCLCPP_INFO_STREAM(getLogger(), "Listening for planning requests on topic " << mplan_req_sub->get_topic_name()); + RCLCPP_INFO_STREAM(getLogger(), "Listening for named constraints on topic " << constr_sub->get_topic_name()); + RCLCPP_INFO_STREAM(getLogger(), "Listening for states on topic " << state_sub->get_topic_name()); rclcpp::spin(node); return 0; diff --git a/moveit_ros/warehouse/src/state_storage.cpp b/moveit_ros/warehouse/src/state_storage.cpp index d85c4c9eb1..bbbfa81587 100644 --- a/moveit_ros/warehouse/src/state_storage.cpp +++ b/moveit_ros/warehouse/src/state_storage.cpp @@ -48,7 +48,7 @@ using warehouse_ros::Metadata; using warehouse_ros::Query; moveit_warehouse::RobotStateStorage::RobotStateStorage(warehouse_ros::DatabaseConnection::Ptr conn) - : MoveItMessageStorage(std::move(conn)), logger_(moveit::make_child_logger("moveit_warehouse_robot_state_storage")) + : MoveItMessageStorage(std::move(conn)), logger_(moveit::makeChildLogger("moveit_warehouse_robot_state_storage")) { createCollections(); } diff --git a/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp b/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp index 79e59d350c..009c162c7a 100644 --- a/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp +++ b/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp @@ -50,7 +50,7 @@ using warehouse_ros::Query; moveit_warehouse::TrajectoryConstraintsStorage::TrajectoryConstraintsStorage(warehouse_ros::DatabaseConnection::Ptr conn) : MoveItMessageStorage(std::move(conn)) - , logger_(moveit::make_child_logger("moveit_warehouse_trajectory_constraints_storage")) + , logger_(moveit::makeChildLogger("moveit_warehouse_trajectory_constraints_storage")) { createCollections(); } diff --git a/moveit_ros/warehouse/src/warehouse_connector.cpp b/moveit_ros/warehouse/src/warehouse_connector.cpp index 99c9e2828a..4c4521f61c 100644 --- a/moveit_ros/warehouse/src/warehouse_connector.cpp +++ b/moveit_ros/warehouse/src/warehouse_connector.cpp @@ -45,11 +45,11 @@ #ifdef _WIN32 void kill(int, int) { - RCLCPP_ERROR(moveit::get_logger(), "Warehouse connector not supported on Windows"); + RCLCPP_ERROR(moveit::getLogger(), "Warehouse connector not supported on Windows"); } // Should never be called int fork() { - RCLCPP_ERROR(moveit::get_logger(), "Warehouse connector not supported on Windows"); + RCLCPP_ERROR(moveit::getLogger(), "Warehouse connector not supported on Windows"); return -1; } #else @@ -59,7 +59,7 @@ int fork() namespace moveit_warehouse { WarehouseConnector::WarehouseConnector(const std::string& dbexec) - : dbexec_(dbexec), child_pid_(0), logger_(moveit::make_child_logger("moveit_warehouse_warehouse_connector")) + : dbexec_(dbexec), child_pid_(0), logger_(moveit::makeChildLogger("moveit_warehouse_warehouse_connector")) { } diff --git a/moveit_ros/warehouse/src/warehouse_services.cpp b/moveit_ros/warehouse/src/warehouse_services.cpp index 1136015636..7234adc9d3 100644 --- a/moveit_ros/warehouse/src/warehouse_services.cpp +++ b/moveit_ros/warehouse/src/warehouse_services.cpp @@ -50,7 +50,7 @@ #include #include -using moveit::get_logger; +using moveit::getLogger; static const std::string ROBOT_DESCRIPTION = "robot_description"; @@ -60,7 +60,7 @@ bool storeState(const std::shared_ptrname.empty()) { - RCLCPP_ERROR(get_logger(), "You must specify a name to store a state"); + RCLCPP_ERROR(getLogger(), "You must specify a name to store a state"); return (response->success = false); } rs.addRobotState(request->state, request->name, request->robot); @@ -96,7 +96,7 @@ bool getState(const std::shared_ptrname, request->robot)) { - RCLCPP_ERROR_STREAM(get_logger(), "No state called '" << request->name << "' for robot '" << request->robot << "'."); + RCLCPP_ERROR_STREAM(getLogger(), "No state called '" << request->name << "' for robot '" << request->robot << "'."); moveit_msgs::msg::RobotState dummy; response->state = dummy; return false; @@ -113,7 +113,7 @@ bool renameState(const std::shared_ptrold_name, request->robot)) { - RCLCPP_ERROR_STREAM(get_logger(), + RCLCPP_ERROR_STREAM(getLogger(), "No state called '" << request->old_name << "' for robot '" << request->robot << "'."); return false; } @@ -127,7 +127,7 @@ bool deleteState(const std::shared_ptrname, request->robot)) { - RCLCPP_ERROR_STREAM(get_logger(), "No state called '" << request->name << "' for robot '" << request->robot << "'."); + RCLCPP_ERROR_STREAM(getLogger(), "No state called '" << request->name << "' for robot '" << request->robot << "'."); return false; } rs.removeRobotState(request->name, request->robot); @@ -141,7 +141,7 @@ int main(int argc, char** argv) node_options.allow_undeclared_parameters(true); node_options.automatically_declare_parameters_from_overrides(true); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("moveit_warehouse_services", node_options); - moveit::get_logger_mut() = node->get_logger(); + moveit::getLoggerMut() = node->get_logger(); std::string host; @@ -161,23 +161,23 @@ int main(int argc, char** argv) conn = moveit_warehouse::loadDatabase(node); conn->setParams(host, port, connection_timeout); - RCLCPP_INFO(get_logger(), "Connecting to warehouse on %s:%d", host.c_str(), port); + RCLCPP_INFO(getLogger(), "Connecting to warehouse on %s:%d", host.c_str(), port); int tries = 0; while (!conn->connect()) { ++tries; - RCLCPP_WARN(get_logger(), "Failed to connect to DB on %s:%d (try %d/%d).", host.c_str(), port, tries, + RCLCPP_WARN(getLogger(), "Failed to connect to DB on %s:%d (try %d/%d).", host.c_str(), port, tries, connection_retries); if (tries == connection_retries) { - RCLCPP_FATAL(get_logger(), "Failed to connect too many times, giving up"); + RCLCPP_FATAL(getLogger(), "Failed to connect too many times, giving up"); return 1; } } } catch (std::exception& ex) { - RCLCPP_ERROR(get_logger(), "%s", ex.what()); + RCLCPP_ERROR(getLogger(), "%s", ex.what()); return 1; } @@ -187,13 +187,13 @@ int main(int argc, char** argv) rs.getKnownRobotStates(names); if (names.empty()) { - RCLCPP_INFO(get_logger(), "There are no previously stored robot states"); + RCLCPP_INFO(getLogger(), "There are no previously stored robot states"); } else { - RCLCPP_INFO(get_logger(), "Previously stored robot states:"); + RCLCPP_INFO(getLogger(), "Previously stored robot states:"); for (const std::string& name : names) - RCLCPP_INFO(get_logger(), " * %s", name.c_str()); + RCLCPP_INFO(getLogger(), " * %s", name.c_str()); } auto save_cb = [&](const std::shared_ptr& request,