diff --git a/.clang-format b/.clang-format index eca6051..9a6ec50 100644 --- a/.clang-format +++ b/.clang-format @@ -1,66 +1,75 @@ --- BasedOnStyle: Google +ColumnLimit: 120 +MaxEmptyLinesToKeep: 1 +SortIncludes: false + +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never AccessModifierOffset: -2 ConstructorInitializerIndentWidth: 2 +NamespaceIndentation: None +ContinuationIndentWidth: 4 +IndentCaseLabels: true +IndentFunctionDeclarationAfterType: false + AlignEscapedNewlinesLeft: false AlignTrailingComments: true + AllowAllParametersOfDeclarationOnNextLine: false +ExperimentalAutoDetectBinPacking: false +ObjCSpaceBeforeProtocolList: true +Cpp11BracedListStyle: false + +AllowShortBlocksOnASingleLine: true AllowShortIfStatementsOnASingleLine: false AllowShortLoopsOnASingleLine: false AllowShortFunctionsOnASingleLine: None -AllowShortLoopsOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false + AlwaysBreakTemplateDeclarations: true AlwaysBreakBeforeMultilineStrings: false BreakBeforeBinaryOperators: false BreakBeforeTernaryOperators: false BreakConstructorInitializersBeforeComma: true + BinPackParameters: true -ColumnLimit: 120 ConstructorInitializerAllOnOneLineOrOnePerLine: true DerivePointerBinding: false PointerBindsToType: true -ExperimentalAutoDetectBinPacking: false -IndentCaseLabels: true -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCSpaceBeforeProtocolList: true -PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 60 -PenaltyBreakString: 1 -PenaltyBreakFirstLessLess: 1000 -PenaltyExcessCharacter: 1000 -PenaltyReturnTypeOnItsOwnLine: 90 + +PenaltyExcessCharacter: 50 +PenaltyBreakBeforeFirstCallParameter: 30 +PenaltyBreakComment: 1000 +PenaltyBreakFirstLessLess: 10 +PenaltyBreakString: 100 +PenaltyReturnTypeOnItsOwnLine: 50 + SpacesBeforeTrailingComments: 2 -Cpp11BracedListStyle: false -Standard: Auto -IndentWidth: 2 -TabWidth: 2 -UseTab: Never -IndentFunctionDeclarationAfterType: false SpacesInParentheses: false SpacesInAngles: false SpaceInEmptyParentheses: false SpacesInCStyleCastParentheses: false +SpaceAfterCStyleCast: false SpaceAfterControlStatementKeyword: true SpaceBeforeAssignmentOperators: true -ContinuationIndentWidth: 4 -SortIncludes: false -SpaceAfterCStyleCast: false # Configure each individual brace in BraceWrapping BreakBeforeBraces: Custom # Control of individual brace wrapping cases -BraceWrapping: { - AfterClass: 'true' - AfterControlStatement: 'true' - AfterEnum : 'true' - AfterFunction : 'true' - AfterNamespace : 'true' - AfterStruct : 'true' - AfterUnion : 'true' - BeforeCatch : 'true' - BeforeElse : 'true' - IndentBraces : 'false' -} +BraceWrapping: + AfterCaseLabel: true + AfterClass: true + AfterControlStatement: true + AfterEnum: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true + AfterUnion: true + BeforeCatch: true + BeforeElse: true + IndentBraces: false ... diff --git a/.travis.yml b/.travis.yml index c2dabb5..a84c12e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -9,13 +9,14 @@ cache: ccache env: global: - - DOCKER_IMAGE=moveit/moveit:master-source - - CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unused-parameter -Wno-unused-function" + - ROS_DISTRO=noetic + - ROS_REPO=ros-testing + - CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls" - WARNINGS_OK=false matrix: - TEST="clang-format catkin_lint" - TEST=clang-tidy-fix - - DOCKER_IMAGE=moveit/moveit:master-source + - DOCKER_IMAGE=moveit/moveit:master-source ROS_REPO= before_script: - git clone -q --depth=1 https://github.com/ros-planning/moveit_ci.git .moveit_ci diff --git a/CMakeLists.txt b/CMakeLists.txt index a86a4b4..7b0544b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -42,13 +42,10 @@ catkin_package( ) ## Build +include_directories(include) include_directories(SYSTEM - ${EIGEN3_INCLUDE_DIRS} -) - -include_directories( - include ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} ) # Visualization Tools Library diff --git a/include/moveit_visual_tools/imarker_end_effector.h b/include/moveit_visual_tools/imarker_end_effector.h index b683b79..d5d9a08 100644 --- a/include/moveit_visual_tools/imarker_end_effector.h +++ b/include/moveit_visual_tools/imarker_end_effector.h @@ -61,8 +61,8 @@ namespace moveit_visual_tools { -using visualization_msgs::InteractiveMarkerFeedback; using visualization_msgs::InteractiveMarkerControl; +using visualization_msgs::InteractiveMarkerFeedback; typedef std::function IMarkerCallback; @@ -177,12 +177,4 @@ typedef std::shared_ptr IMarkerEndEffectorPtr; typedef std::shared_ptr IMarkerEndEffectorConstPtr; } // namespace moveit_visual_tools -namespace -{ -/** \brief Collision checking handle for IK solvers */ -bool isStateValid(const planning_scene::PlanningScene* planning_scene, bool verbose, bool only_check_self_collision, - const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools_, moveit::core::RobotState* state, - const moveit::core::JointModelGroup* group, const double* ik_solution); -} - #endif // MOVEIT_VISUAL_TOOLS_IMARKER_END_EFFECTOR_H diff --git a/include/moveit_visual_tools/imarker_robot_state.h b/include/moveit_visual_tools/imarker_robot_state.h index 2d7b99c..a82959d 100644 --- a/include/moveit_visual_tools/imarker_robot_state.h +++ b/include/moveit_visual_tools/imarker_robot_state.h @@ -54,8 +54,8 @@ namespace moveit_visual_tools { -using visualization_msgs::InteractiveMarkerFeedback; using visualization_msgs::InteractiveMarkerControl; +using visualization_msgs::InteractiveMarkerFeedback; typedef std::function IMarkerCallback; @@ -182,12 +182,4 @@ typedef std::shared_ptr IMarkerRobotStatePtr; typedef std::shared_ptr IMarkerRobotStateConstPtr; } // namespace moveit_visual_tools -namespace -{ -/** \brief Collision checking handle for IK solvers */ -bool isIKStateValid(const planning_scene::PlanningScene* planning_scene, bool verbose, bool only_check_self_collision, - const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools_, moveit::core::RobotState* state, - const moveit::core::JointModelGroup* group, const double* ik_solution); -} - #endif // MOVEIT_VISUAL_TOOLS_IMARKER_ROBOT_STATE_H diff --git a/src/imarker_end_effector.cpp b/src/imarker_end_effector.cpp index b9013f0..b9b91cb 100644 --- a/src/imarker_end_effector.cpp +++ b/src/imarker_end_effector.cpp @@ -49,6 +49,61 @@ #include #include +namespace +{ +bool isStateValid(const planning_scene::PlanningScene* planning_scene, bool verbose, bool only_check_self_collision, + const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, moveit::core::RobotState* robot_state, + const moveit::core::JointModelGroup* group, const double* ik_solution) +{ + // Apply IK solution to robot state + robot_state->setJointGroupPositions(group, ik_solution); + robot_state->update(); + +#if 0 // Ensure there are objects in the planning scene + const std::size_t num_collision_objects = planning_scene->getCollisionEnv()->getWorld()->size(); + if (num_collision_objects == 0) + { + ROS_ERROR_STREAM_NAMED("cart_path_planner", "No collision objects exist in world, you need at least a table " + "modeled for the controller to work"); + ROS_ERROR_STREAM_NAMED("cart_path_planner", "To fix this, relaunch the teleop/head tracking/whatever MoveIt " + "node to publish the collision objects"); + return false; + } +#endif + + if (!planning_scene) + { + ROS_ERROR_STREAM_NAMED("cart_path_planner", "No planning scene provided"); + return false; + } + if (only_check_self_collision) + { + // No easy API exists for only checking self-collision, so we do it here. + // TODO(davetcoleman): move this into planning_scene.cpp + collision_detection::CollisionRequest req; + req.verbose = verbose; + req.group_name = group->getName(); + collision_detection::CollisionResult res; + planning_scene->checkSelfCollision(req, res, *robot_state); + if (!res.collision) + return true; // not in collision + } + else if (!planning_scene->isStateColliding(*robot_state, group->getName())) + return true; // not in collision + + // Display more info about the collision + if (verbose) + { + visual_tools->publishRobotState(*robot_state, rviz_visual_tools::RED); + planning_scene->isStateColliding(*robot_state, group->getName(), true); + visual_tools->publishContactPoints(*robot_state, planning_scene); + } + ROS_WARN_STREAM_THROTTLE_NAMED(2.0, "cart_path_planner", "Collision"); + return false; +} + +} // namespace + namespace moveit_visual_tools { IMarkerEndEffector::IMarkerEndEffector(IMarkerRobotState* imarker_parent, const std::string& imarker_name, @@ -174,7 +229,7 @@ void IMarkerEndEffector::initializeInteractiveMarkers() make6DofMarker(pose_msg); } -void IMarkerEndEffector::updateIMarkerPose(const Eigen::Isometry3d& pose) +void IMarkerEndEffector::updateIMarkerPose(const Eigen::Isometry3d& /*pose*/) { // Move marker to tip of fingers // imarker_pose_ = pose * imarker_offset_.inverse(); @@ -269,58 +324,3 @@ IMarkerEndEffector::makeBoxControl(visualization_msgs::InteractiveMarker& msg) } } // namespace moveit_visual_tools - -namespace -{ -bool isStateValid(const planning_scene::PlanningScene* planning_scene, bool verbose, bool only_check_self_collision, - const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, moveit::core::RobotState* robot_state, - const moveit::core::JointModelGroup* group, const double* ik_solution) -{ - // Apply IK solution to robot state - robot_state->setJointGroupPositions(group, ik_solution); - robot_state->update(); - -#if 0 // Ensure there are objects in the planning scene - const std::size_t num_collision_objects = planning_scene->getCollisionEnv()->getWorld()->size(); - if (num_collision_objects == 0) - { - ROS_ERROR_STREAM_NAMED("cart_path_planner", "No collision objects exist in world, you need at least a table " - "modeled for the controller to work"); - ROS_ERROR_STREAM_NAMED("cart_path_planner", "To fix this, relaunch the teleop/head tracking/whatever MoveIt " - "node to publish the collision objects"); - return false; - } -#endif - - if (!planning_scene) - { - ROS_ERROR_STREAM_NAMED("cart_path_planner", "No planning scene provided"); - return false; - } - if (only_check_self_collision) - { - // No easy API exists for only checking self-collision, so we do it here. - // TODO(davetcoleman): move this into planning_scene.cpp - collision_detection::CollisionRequest req; - req.verbose = verbose; - req.group_name = group->getName(); - collision_detection::CollisionResult res; - planning_scene->checkSelfCollision(req, res, *robot_state); - if (!res.collision) - return true; // not in collision - } - else if (!planning_scene->isStateColliding(*robot_state, group->getName())) - return true; // not in collision - - // Display more info about the collision - if (verbose) - { - visual_tools->publishRobotState(*robot_state, rviz_visual_tools::RED); - planning_scene->isStateColliding(*robot_state, group->getName(), true); - visual_tools->publishContactPoints(*robot_state, planning_scene); - } - ROS_WARN_STREAM_THROTTLE_NAMED(2.0, "cart_path_planner", "Collision"); - return false; -} - -} // namespace diff --git a/src/imarker_robot_state.cpp b/src/imarker_robot_state.cpp index 96de8ec..cfa1c44 100644 --- a/src/imarker_robot_state.cpp +++ b/src/imarker_robot_state.cpp @@ -49,6 +49,63 @@ #include #include +namespace +{ +bool isIKStateValid(const planning_scene::PlanningScene* planning_scene, bool verbose, bool only_check_self_collision, + const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, + moveit::core::RobotState* robot_state, const moveit::core::JointModelGroup* group, + const double* ik_solution) +{ + // Apply IK solution to robot state + robot_state->setJointGroupPositions(group, ik_solution); + robot_state->update(); + +#if 0 // Ensure there are objects in the planning scene + const std::size_t num_collision_objects = planning_scene->getCollisionEnv()->getWorld()->size(); + if (num_collision_objects == 0) + { + ROS_ERROR_STREAM_NAMED("imarker_robot_state", "No collision objects exist in world, you need at least a table " + "modeled for the controller to work"); + ROS_ERROR_STREAM_NAMED("imarker_robot_state", "To fix this, relaunch the teleop/head tracking/whatever MoveIt " + "node to publish the collision objects"); + return false; + } +#endif + + if (!planning_scene) + { + ROS_ERROR_STREAM_NAMED("imarker_robot_state", "No planning scene provided"); + return false; + } + if (only_check_self_collision) + { + // No easy API exists for only checking self-collision, so we do it here. + // TODO(davetcoleman): move this into planning_scene.cpp + collision_detection::CollisionRequest req; + req.verbose = verbose; + req.group_name = group->getName(); + collision_detection::CollisionResult res; + planning_scene->checkSelfCollision(req, res, *robot_state); + if (!res.collision) + return true; // not in collision + } + else if (!planning_scene->isStateColliding(*robot_state, group->getName())) + return true; // not in collision + + // Display more info about the collision + if (verbose) + { + visual_tools->publishRobotState(*robot_state, rviz_visual_tools::RED); + planning_scene->isStateColliding(*robot_state, group->getName(), true); + visual_tools->publishContactPoints(*robot_state, planning_scene); + ROS_WARN_STREAM_THROTTLE_NAMED(2.0, "imarker_robot_state", "Collision in IK CC callback"); + } + + return false; +} + +} // namespace + namespace moveit_visual_tools { IMarkerRobotState::IMarkerRobotState(planning_scene_monitor::PlanningSceneMonitorPtr psm, @@ -221,8 +278,8 @@ bool IMarkerRobotState::setToRandomState(double clearance) ROS_WARN_STREAM_NAMED(name_, "Taking long time to find valid random state"); } - ROS_ERROR_STREAM_NAMED(name_, "Unable to find valid random robot state for imarker after " << MAX_ATTEMPTS << " attem" - "pts"); + ROS_ERROR_STREAM_NAMED(name_, + "Unable to find valid random robot state for imarker after " << MAX_ATTEMPTS << " attempts"); return false; } @@ -332,60 +389,3 @@ bool IMarkerRobotState::setFromPoses(const EigenSTL::vector_Isometry3d& poses, } } // namespace moveit_visual_tools - -namespace -{ -bool isIKStateValid(const planning_scene::PlanningScene* planning_scene, bool verbose, bool only_check_self_collision, - const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, - moveit::core::RobotState* robot_state, const moveit::core::JointModelGroup* group, - const double* ik_solution) -{ - // Apply IK solution to robot state - robot_state->setJointGroupPositions(group, ik_solution); - robot_state->update(); - -#if 0 // Ensure there are objects in the planning scene - const std::size_t num_collision_objects = planning_scene->getCollisionEnv()->getWorld()->size(); - if (num_collision_objects == 0) - { - ROS_ERROR_STREAM_NAMED("imarker_robot_state", "No collision objects exist in world, you need at least a table " - "modeled for the controller to work"); - ROS_ERROR_STREAM_NAMED("imarker_robot_state", "To fix this, relaunch the teleop/head tracking/whatever MoveIt " - "node to publish the collision objects"); - return false; - } -#endif - - if (!planning_scene) - { - ROS_ERROR_STREAM_NAMED("imarker_robot_state", "No planning scene provided"); - return false; - } - if (only_check_self_collision) - { - // No easy API exists for only checking self-collision, so we do it here. - // TODO(davetcoleman): move this into planning_scene.cpp - collision_detection::CollisionRequest req; - req.verbose = verbose; - req.group_name = group->getName(); - collision_detection::CollisionResult res; - planning_scene->checkSelfCollision(req, res, *robot_state); - if (!res.collision) - return true; // not in collision - } - else if (!planning_scene->isStateColliding(*robot_state, group->getName())) - return true; // not in collision - - // Display more info about the collision - if (verbose) - { - visual_tools->publishRobotState(*robot_state, rviz_visual_tools::RED); - planning_scene->isStateColliding(*robot_state, group->getName(), true); - visual_tools->publishContactPoints(*robot_state, planning_scene); - ROS_WARN_STREAM_THROTTLE_NAMED(2.0, "imarker_robot_state", "Collision in IK CC callback"); - } - - return false; -} - -} // namespace diff --git a/src/moveit_visual_tools.cpp b/src/moveit_visual_tools.cpp index 778382a..445f202 100644 --- a/src/moveit_visual_tools.cpp +++ b/src/moveit_visual_tools.cpp @@ -606,7 +606,7 @@ bool MoveItVisualTools::cleanupCO(const std::string& name) return processCollisionObjectMsg(co); } -bool MoveItVisualTools::cleanupACO(const std::string& name) +bool MoveItVisualTools::cleanupACO(const std::string& /*name*/) { // Clean up old attached collision object moveit_msgs::AttachedCollisionObject aco; @@ -647,7 +647,7 @@ bool MoveItVisualTools::publishCollisionBlock(const geometry_msgs::Pose& block_p collision_obj.primitives.resize(1); collision_obj.primitives[0].type = shape_msgs::SolidPrimitive::BOX; collision_obj.primitives[0].dimensions.resize( - geometric_shapes::SolidPrimitiveDimCount::value); + geometric_shapes::solidPrimitiveDimCount()); collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = block_size; collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = block_size; collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = block_size; @@ -684,7 +684,7 @@ bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Point& point collision_obj.primitives.resize(1); collision_obj.primitives[0].type = shape_msgs::SolidPrimitive::BOX; collision_obj.primitives[0].dimensions.resize( - geometric_shapes::SolidPrimitiveDimCount::value); + geometric_shapes::solidPrimitiveDimCount()); collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = fabs(point1.x - point2.x); collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = fabs(point1.y - point2.y); collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = fabs(point1.z - point2.z); @@ -726,7 +726,7 @@ bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Pose& pose, collision_obj.primitives.resize(1); collision_obj.primitives[0].type = shape_msgs::SolidPrimitive::BOX; collision_obj.primitives[0].dimensions.resize( - geometric_shapes::SolidPrimitiveDimCount::value); + geometric_shapes::solidPrimitiveDimCount()); collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = width; collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = depth; collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = height; @@ -806,7 +806,7 @@ bool MoveItVisualTools::publishCollisionCylinder(const geometry_msgs::Pose& obje collision_obj.primitives.resize(1); collision_obj.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER; collision_obj.primitives[0].dimensions.resize( - geometric_shapes::SolidPrimitiveDimCount::value); + geometric_shapes::solidPrimitiveDimCount()); collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT] = height; collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] = radius; collision_obj.primitive_poses.resize(1); @@ -918,8 +918,7 @@ bool MoveItVisualTools::publishCollisionGraph(const graph_msgs::GeometryGraph& g // Create the solid primitive shape_msgs::SolidPrimitive cylinder; cylinder.type = shape_msgs::SolidPrimitive::CYLINDER; - cylinder.dimensions.resize( - geometric_shapes::SolidPrimitiveDimCount::value); + cylinder.dimensions.resize(geometric_shapes::solidPrimitiveDimCount()); cylinder.dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT] = height; cylinder.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] = radius; @@ -944,7 +943,7 @@ void MoveItVisualTools::getCollisionWallMsg(double x, double y, double z, double collision_obj.primitives.resize(1); collision_obj.primitives[0].type = shape_msgs::SolidPrimitive::BOX; collision_obj.primitives[0].dimensions.resize( - geometric_shapes::SolidPrimitiveDimCount::value); + geometric_shapes::solidPrimitiveDimCount()); geometry_msgs::Pose rec_pose; @@ -1016,7 +1015,7 @@ bool MoveItVisualTools::publishCollisionTable(double x, double y, double z, doub collision_obj.primitives.resize(1); collision_obj.primitives[0].type = shape_msgs::SolidPrimitive::BOX; collision_obj.primitives[0].dimensions.resize( - geometric_shapes::SolidPrimitiveDimCount::value); + geometric_shapes::solidPrimitiveDimCount()); // Size collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = depth;