diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 000000000..7c778928b --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,36 @@ +--- +Checks: '-*, + performance-*, + llvm-namespace-comment, + modernize-redundant-void-arg, + modernize-use-nullptr, + modernize-use-default, + modernize-use-override, + modernize-loop-convert, + readability-named-parameter, + readability-redundant-smartptr-get, + readability-redundant-string-cstr, + readability-simplify-boolean-expr, + readability-container-size-empty, + readability-identifier-naming, + ' +HeaderFilterRegex: '' +AnalyzeTemporaryDtors: false +CheckOptions: + - key: llvm-namespace-comment.ShortNamespaceLines + value: '10' + - key: llvm-namespace-comment.SpacesBeforeComments + value: '2' + - key: readability-braces-around-statements.ShortStatementLines + value: '2' + # type names + - key: readability-identifier-naming.ClassCase + value: CamelCase + - key: readability-identifier-naming.EnumCase + value: CamelCase + - key: readability-identifier-naming.UnionCase + value: CamelCase + # method names + - key: readability-identifier-naming.MethodCase + value: camelBack +... diff --git a/.travis.yml b/.travis.yml index 6a09bc37c..f56c8920a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -6,55 +6,27 @@ rvm: - 2.4 python: - "2.7" -cache: ccache -compiler: gcc env: global: - - ROS_DISTRO=melodic - - DOCKER_IMAGE=moveit/moveit:master-source + - SCRIPT=.moveit_ci/travis.sh + - ROS_DISTRO=noetic + - ROS_REPO=ros-testing + - CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls" + - WARNINGS_OK=false + - UPSTREAM_WORKSPACE="https://github.com/ros-planning/moveit_visual_tools + https://github.com/ros-planning/panda_moveit_config" matrix: - - TEST="clang-format" - - ROS_DISTRO=melodic - -before_install: # Use this to prepare the system to install prerequisites or dependencies - # Define some config vars - - export NOKOGIRI_USE_SYSTEM_LIBRARIES=true - - export REPOSITORY_NAME=${PWD##*/} - - echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME" - - sudo -E sh -c 'echo "deb http://packages.ros.org/ros/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list' - - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - - - sudo apt-get update -qq - - sudo apt-get install -qq -y python-rosdep python-wstool python-catkin-tools - # Setup rosdep - - sudo rosdep init - - rosdep update - # Install htmlpoofer - - gem update --system - - gem --version - - gem install html-proofer - # Install ROS's version of sphinx - - sudo apt-get -qq install ros-kinetic-rosdoc-lite - - source /opt/ros/kinetic/setup.bash + - SCRIPT=htmlproofer.sh + - TEST="clang-format catkin_lint" + - TEST="clang-tidy-fix" + - 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 script: - # Build tutorial examples - - .moveit_ci/travis.sh - # Test build with non-ROS wrapped Sphinx command to allow warnings and errors to be caught - - sphinx-build -W -b html . native_build - # Test build with ROS-version of Sphinx command so that it is generated same as ros.org - - rosdoc_lite -o build . - # Run HTML tests on generated build output to check for 404 errors, etc - - test "$TRAVIS_PULL_REQUEST" == false || URL_SWAP="--url-swap https\://github.com/ros-planning/moveit_tutorials/blob/master/:file\://$PWD/build/html/" - - htmlproofer ./build --only-4xx --check-html --file-ignore ./build/html/genindex.html,./build/html/search.html --alt-ignore '/.*/' --url-ignore '#' $URL_SWAP - -# after_success and deploy are skipped if build is broken -after_success: - # Tell GitHub Pages not to bypass Jekyll processing - - touch build/html/.nojekyll + ./$SCRIPT deploy: # Deploy to gh-pages branch @@ -66,5 +38,5 @@ deploy: # Only copy build output directory to gh-pages local_dir: build/html on: - # Source branch branch: master + condition: $SCRIPT = htmlproofer.sh diff --git a/CMakeLists.txt b/CMakeLists.txt index 30540defa..731b1dae0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,10 +8,11 @@ find_package(catkin REQUIRED COMPONENTS interactive_markers moveit_core - moveit_visual_tools moveit_ros_planning moveit_ros_planning_interface moveit_ros_perception + rviz_visual_tools + moveit_visual_tools pluginlib geometric_shapes pcl_ros @@ -46,7 +47,8 @@ catkin_package( ## Build ## ########### -include_directories(${THIS_PACKAGE_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIRS}) +include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIRS}) add_subdirectory(doc/kinematics) add_subdirectory(doc/robot_model_and_robot_state) diff --git a/_static/logo.png b/_static/logo.png old mode 100755 new mode 100644 diff --git a/doc/controller_configuration/src/moveit_controller_manager_example.cpp b/doc/controller_configuration/src/moveit_controller_manager_example.cpp index 86a1b79b3..9f3c2fb9d 100644 --- a/doc/controller_configuration/src/moveit_controller_manager_example.cpp +++ b/doc/controller_configuration/src/moveit_controller_manager_example.cpp @@ -49,7 +49,7 @@ class ExampleControllerHandle : public moveit_controller_manager::MoveItControll { } - bool sendTrajectory(const moveit_msgs::RobotTrajectory& t) override + bool sendTrajectory(const moveit_msgs::RobotTrajectory& /*t*/) override { // do whatever is needed to actually execute this trajectory return true; @@ -134,7 +134,7 @@ class MoveItControllerManagerExample : public moveit_controller_manager::MoveItC * Controllers are all active and default. */ moveit_controller_manager::MoveItControllerManager::ControllerState - getControllerState(const std::string& name) override + getControllerState(const std::string& /*name*/) override { moveit_controller_manager::MoveItControllerManager::ControllerState state; state.active_ = true; @@ -143,7 +143,8 @@ class MoveItControllerManagerExample : public moveit_controller_manager::MoveItC } /* Cannot switch our controllers */ - bool switchControllers(const std::vector& activate, const std::vector& deactivate) override + bool switchControllers(const std::vector& /*activate*/, + const std::vector& /*deactivate*/) override { return false; } diff --git a/doc/creating_moveit_plugins/lerp_motion_planner/CMakeLists.txt b/doc/creating_moveit_plugins/lerp_motion_planner/CMakeLists.txt index cbafdcbbf..9a274b9d2 100644 --- a/doc/creating_moveit_plugins/lerp_motion_planner/CMakeLists.txt +++ b/doc/creating_moveit_plugins/lerp_motion_planner/CMakeLists.txt @@ -1,18 +1,10 @@ +include_directories(include) include_directories( - include - ${catkin_INCLUDE_DIRS} SYSTEM + ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ) -add_library(${PROJECT_NAME} - src/lerp_interface.cpp - src/lerp_planning_context.cpp -) - -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -set_target_properties(${PROJECT_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") - add_executable(lerp_example src/lerp_example.cpp) target_link_libraries(lerp_example ${catkin_LIBRARIES} @@ -20,10 +12,12 @@ target_link_libraries(lerp_example ) # Lerp planning plugin -add_library(moveit_lerp_planner_plugin src/lerp_planner_manager.cpp) +add_library(moveit_lerp_planner_plugin + src/lerp_planner_manager.cpp + src/lerp_interface.cpp + src/lerp_planning_context.cpp) set_target_properties(moveit_lerp_planner_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") - -target_link_libraries(moveit_lerp_planner_plugin ${PROJECT_NAME} ${catkin_LIBRARIES}) +target_link_libraries(moveit_lerp_planner_plugin ${catkin_LIBRARIES} ${Boost_LIBRARIES}) ############# ## Install ## @@ -56,6 +50,6 @@ install( DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) - +#catkin_lint ignore uninstalled_plugin install(FILES lerp_interface_plugin_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/doc/creating_moveit_plugins/lerp_motion_planner/) diff --git a/doc/creating_moveit_plugins/lerp_motion_planner/include/lerp_interface/lerp_interface.h b/doc/creating_moveit_plugins/lerp_motion_planner/include/lerp_interface/lerp_interface.h index c72d95ab8..657d027ef 100644 --- a/doc/creating_moveit_plugins/lerp_motion_planner/include/lerp_interface/lerp_interface.h +++ b/doc/creating_moveit_plugins/lerp_motion_planner/include/lerp_interface/lerp_interface.h @@ -59,7 +59,7 @@ class LERPInterface int dof_; private: - void interpolate(const std::vector joint_names, moveit::core::RobotStatePtr& robot_state, + void interpolate(const std::vector& joint_names, moveit::core::RobotStatePtr& robot_state, const moveit::core::JointModelGroup* joint_model_group, const std::vector& start_joint_vals, const std::vector& goal_joint_vals, trajectory_msgs::JointTrajectory& joint_trajectory); }; diff --git a/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_example.cpp b/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_example.cpp index 1da6856bb..c28546e86 100644 --- a/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_example.cpp +++ b/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_example.cpp @@ -85,7 +85,6 @@ int main(int argc, char** argv) // Create JointModelGroup const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP); - const std::vector& joint_names = joint_model_group->getActiveJointModelNames(); const std::vector& link_model_names = joint_model_group->getLinkModelNames(); ROS_INFO_NAMED(NODE_NAME, "end effector name %s\n", link_model_names.back().c_str()); @@ -183,9 +182,9 @@ int main(int argc, char** argv) std::vector solution_positions; solution_positions = solution_traj.joint_trajectory.points[step_num].positions; std::stringstream sst; - for (int i = 0; i < solution_positions.size(); ++i) + for (double solution_position : solution_positions) { - sst << solution_positions[i] << " "; + sst << solution_position << " "; } ROS_INFO_STREAM_NAMED(NODE_NAME, sst.str()); } diff --git a/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_interface.cpp b/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_interface.cpp index 8c4a27bc7..d2b073919 100644 --- a/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_interface.cpp +++ b/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_interface.cpp @@ -77,7 +77,9 @@ bool LERPInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_ std::vector goal_joint_constraint = goal_constraints[0].joint_constraints; std::vector goal_joint_values; - for (auto constraint : goal_joint_constraint) + goal_joint_values.reserve(goal_joint_constraint.size()); + + for (const auto& constraint : goal_joint_constraint) { goal_joint_values.push_back(constraint.position); } @@ -102,7 +104,7 @@ bool LERPInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_ return true; } -void LERPInterface::interpolate(const std::vector joint_names, moveit::core::RobotStatePtr& rob_state, +void LERPInterface::interpolate(const std::vector& joint_names, moveit::core::RobotStatePtr& rob_state, const moveit::core::JointModelGroup* joint_model_group, const std::vector& start_joint_vals, const std::vector& goal_joint_vals, trajectory_msgs::JointTrajectory& joint_trajectory) diff --git a/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_planner_manager.cpp b/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_planner_manager.cpp index 921acc74e..c8175fa06 100644 --- a/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_planner_manager.cpp +++ b/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_planner_manager.cpp @@ -52,7 +52,7 @@ class LERPPlannerManager : public planning_interface::PlannerManager { } - bool initialize(const moveit::core::RobotModelConstPtr& model, const std::string& ns) override + bool initialize(const moveit::core::RobotModelConstPtr& model, const std::string& /*ns*/) override { for (const std::string& gpName : model->getJointModelGroupNames()) { diff --git a/doc/interactivity/include/interactivity/imarker.h b/doc/interactivity/include/interactivity/imarker.h index 5939bb0f4..5d84bbeef 100644 --- a/doc/interactivity/include/interactivity/imarker.h +++ b/doc/interactivity/include/interactivity/imarker.h @@ -41,6 +41,7 @@ #include #include +#include /* Interactive marker. * @@ -69,7 +70,8 @@ class IMarker Dof dof = BOTH) : imarker_() { - initialize(server, name, Eigen::Vector3d(0, 0, 0), Eigen::Quaterniond(1, 0, 0, 0), frame_id, callback, dof); + initialize(server, name, Eigen::Vector3d(0, 0, 0), Eigen::Quaterniond(1, 0, 0, 0), frame_id, std::move(callback), + dof); } /** create an interactive marker with an initial pose */ @@ -82,7 +84,7 @@ class IMarker Eigen::Quaterniond q(pose.linear()); Eigen::Vector3d p = pose.translation(); - initialize(server, name, p, q, frame_id, callback, dof); + initialize(server, name, p, q, frame_id, std::move(callback), dof); } /** create an interactive marker with an initial pose */ @@ -93,7 +95,7 @@ class IMarker Dof dof = BOTH) : imarker_() { - initialize(server, name, position, orientation, frame_id, callback, dof); + initialize(server, name, position, orientation, frame_id, std::move(callback), dof); } /** create an interactive marker with an initial position */ @@ -103,14 +105,14 @@ class IMarker Dof dof = BOTH) : imarker_() { - initialize(server, name, position, Eigen::Quaterniond(1, 0, 0, 0), frame_id, callback, dof); + initialize(server, name, position, Eigen::Quaterniond(1, 0, 0, 0), frame_id, std::move(callback), dof); } /** move marker to new pose */ void move(const Eigen::Isometry3d& pose); /** default callback which just prints new position and orientation */ - static void printFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr&); + static void printFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& /*feedback*/); private: /* called by constructors */ diff --git a/doc/interactivity/include/interactivity/interactive_robot.h b/doc/interactivity/include/interactivity/interactive_robot.h index f6896cff1..d4cd93b1c 100644 --- a/doc/interactivity/include/interactivity/interactive_robot.h +++ b/doc/interactivity/include/interactivity/interactive_robot.h @@ -44,6 +44,9 @@ #include #include #include + +#include + #include "imarker.h" /** Keeps track of the state of the robot and the world. @@ -71,7 +74,7 @@ class InteractiveRobot /** set a callback to call when updates occur */ void setUserCallback(boost::function callback) { - user_callback_ = callback; + user_callback_ = std::move(callback); } /** access RobotModel */ diff --git a/doc/interactivity/src/imarker.cpp b/doc/interactivity/src/imarker.cpp index 7041ca357..80a390c2e 100644 --- a/doc/interactivity/src/imarker.cpp +++ b/doc/interactivity/src/imarker.cpp @@ -40,6 +40,8 @@ #include "interactivity/pose_string.h" #include +#include + /* default callback which just prints the current pose */ void IMarker::printFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) { @@ -182,6 +184,6 @@ void IMarker::initialize(interactive_markers::InteractiveMarkerServer& server, c // tell the server to show the marker and listen for changes server_->insert(imarker_); - server_->setCallback(imarker_.name, callback); + server_->setCallback(imarker_.name, std::move(callback)); server_->applyChanges(); } diff --git a/doc/interactivity/src/interactive_robot.cpp b/doc/interactivity/src/interactive_robot.cpp index 2a2e88684..9d38fa5fb 100644 --- a/doc/interactivity/src/interactive_robot.cpp +++ b/doc/interactivity/src/interactive_robot.cpp @@ -52,23 +52,18 @@ const ros::Duration InteractiveRobot::min_delay_(0.01); InteractiveRobot::InteractiveRobot(const std::string& robot_description, const std::string& robot_topic, const std::string& marker_topic, const std::string& imarker_topic) - : // this node handle is used to create the publishers - nh_() - , + : user_data_(nullptr) + , nh_() // this node handle is used to create the publishers // create publishers for markers and robot state - robot_state_publisher_(nh_.advertise(robot_topic, 1)) + , robot_state_publisher_(nh_.advertise(robot_topic, 1)) , world_state_publisher_(nh_.advertise(marker_topic, 100)) - , // create an interactive marker server for displaying interactive markers - interactive_marker_server_(imarker_topic) - , imarker_robot_(0) - , imarker_world_(0) - , + , interactive_marker_server_(imarker_topic) + , imarker_robot_(nullptr) + , imarker_world_(nullptr) // load the robot description - rm_loader_(robot_description) - , group_(0) - , user_data_(0) - , user_callback_(0) + , rm_loader_(robot_description) + , group_(nullptr) { // get the RobotModel loaded from urdf and srdf files robot_model_ = rm_loader_.getModel(); @@ -209,7 +204,7 @@ void InteractiveRobot::scheduleUpdate() } /* callback called when it is time to publish */ -void InteractiveRobot::updateCallback(const ros::TimerEvent& e) +void InteractiveRobot::updateCallback(const ros::TimerEvent& /*event*/) { ros::Time tbegin = ros::Time::now(); publish_timer_.stop(); diff --git a/doc/kinematics/src/ros_api_tutorial.cpp b/doc/kinematics/src/ros_api_tutorial.cpp index bf46b0607..59a3621cb 100644 --- a/doc/kinematics/src/ros_api_tutorial.cpp +++ b/doc/kinematics/src/ros_api_tutorial.cpp @@ -89,7 +89,7 @@ int main(int argc, char** argv) /* Filling in a seed state */ robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); - moveit::core::RobotModelPtr kinematic_model = robot_model_loader.getModel(); + const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel(); moveit::core::RobotStatePtr kinematic_state(new moveit::core::RobotState(kinematic_model)); const moveit::core::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("panda_arm"); diff --git a/doc/motion_planning_api/src/motion_planning_api_tutorial.cpp b/doc/motion_planning_api/src/motion_planning_api_tutorial.cpp index 536d49dc3..debce2d21 100644 --- a/doc/motion_planning_api/src/motion_planning_api_tutorial.cpp +++ b/doc/motion_planning_api/src/motion_planning_api_tutorial.cpp @@ -72,7 +72,7 @@ int main(int argc, char** argv) // http://docs.ros.org/indigo/api/moveit_ros_planning/html/classrobot__model__loader_1_1RobotModelLoader.html const std::string PLANNING_GROUP = "panda_arm"; robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); - moveit::core::RobotModelPtr robot_model = robot_model_loader.getModel(); + const moveit::core::RobotModelPtr& robot_model = robot_model_loader.getModel(); /* Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group*/ moveit::core::RobotStatePtr robot_state(new moveit::core::RobotState(robot_model)); const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP); @@ -115,8 +115,8 @@ int main(int argc, char** argv) { const std::vector& classes = planner_plugin_loader->getDeclaredClasses(); std::stringstream ss; - for (std::size_t i = 0; i < classes.size(); ++i) - ss << classes[i] << " "; + for (const auto& cls : classes) + ss << cls << " "; ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl << "Available plugins: " << ss.str()); } diff --git a/doc/perception_pipeline/src/bag_publisher_maintain_time.cpp b/doc/perception_pipeline/src/bag_publisher_maintain_time.cpp index dbdece7d1..c813478dd 100644 --- a/doc/perception_pipeline/src/bag_publisher_maintain_time.cpp +++ b/doc/perception_pipeline/src/bag_publisher_maintain_time.cpp @@ -63,7 +63,7 @@ int main(int argc, char** argv) for (auto const msg : bagtopics_iter) { sensor_msgs::PointCloud2::Ptr point_cloud_ptr = msg.instantiate(); - if (point_cloud_ptr == NULL) + if (point_cloud_ptr == nullptr) { std::cout << "error" << std::endl; break; diff --git a/doc/perception_pipeline/src/cylinder_segment.cpp b/doc/perception_pipeline/src/cylinder_segment.cpp index 94f9d313d..99a9576d3 100644 --- a/doc/perception_pipeline/src/cylinder_segment.cpp +++ b/doc/perception_pipeline/src/cylinder_segment.cpp @@ -112,13 +112,13 @@ class CylinderSegment cylinder_params. @param cloud - Pointcloud containing just the cylinder. @param cylinder_params - Pointer to the struct AddCylinderParams. */ - void extractLocationHeight(pcl::PointCloud::Ptr cloud) + void extractLocationHeight(const pcl::PointCloud::Ptr& cloud) { - double max_angle_y = 0.0; + double max_angle_y = -std::numeric_limits::infinity(); double min_angle_y = std::numeric_limits::infinity(); - double lowest_point[3]; - double highest_point[3]; + double lowest_point[3] = { 0.0, 0.0, 0.0 }; + double highest_point[3] = { 0.0, 0.0, 0.0 }; // BEGIN_SUB_TUTORIAL extract_location_height // Consider a point inside the point cloud and imagine that point is formed on a XY plane where the perpendicular // distance from the plane to the camera is Z. |br| @@ -135,18 +135,19 @@ class CylinderSegment // Loop over the entire pointcloud. for (auto const point : cloud->points) { + const double angle = atan2(point.z, point.y); /* Find the coordinates of the highest point */ - if (atan2(point.z, point.y) < min_angle_y) + if (angle < min_angle_y) { - min_angle_y = atan2(point.z, point.y); + min_angle_y = angle; lowest_point[0] = point.x; lowest_point[1] = point.y; lowest_point[2] = point.z; } /* Find the coordinates of the lowest point */ - else if (atan2(point.z, point.y) > max_angle_y) + else if (angle > max_angle_y) { - max_angle_y = atan2(point.z, point.y); + max_angle_y = angle; highest_point[0] = point.x; highest_point[1] = point.y; highest_point[2] = point.z; @@ -165,7 +166,7 @@ class CylinderSegment /** \brief Given a pointcloud extract the ROI defined by the user. @param cloud - Pointcloud whose ROI needs to be extracted. */ - void passThroughFilter(pcl::PointCloud::Ptr cloud) + void passThroughFilter(const pcl::PointCloud::Ptr& cloud) { pcl::PassThrough pass; pass.setInputCloud(cloud); @@ -178,7 +179,8 @@ class CylinderSegment /** \brief Given the pointcloud and pointer cloud_normals compute the point normals and store in cloud_normals. @param cloud - Pointcloud. @param cloud_normals - The point normals once computer will be stored in this. */ - void computeNormals(pcl::PointCloud::Ptr cloud, pcl::PointCloud::Ptr cloud_normals) + void computeNormals(const pcl::PointCloud::Ptr& cloud, + const pcl::PointCloud::Ptr& cloud_normals) { pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); pcl::NormalEstimation ne; @@ -192,7 +194,8 @@ class CylinderSegment /** \brief Given the point normals and point indices, extract the normals for the indices. @param cloud_normals - Point normals. @param inliers_plane - Indices whose normals need to be extracted. */ - void extractNormals(pcl::PointCloud::Ptr cloud_normals, pcl::PointIndices::Ptr inliers_plane) + void extractNormals(const pcl::PointCloud::Ptr& cloud_normals, + const pcl::PointIndices::Ptr& inliers_plane) { pcl::ExtractIndices extract_normals; extract_normals.setNegative(true); @@ -204,7 +207,8 @@ class CylinderSegment /** \brief Given the pointcloud and indices of the plane, remove the plannar region from the pointcloud. @param cloud - Pointcloud. @param inliers_plane - Indices representing the plane. */ - void removePlaneSurface(pcl::PointCloud::Ptr cloud, pcl::PointIndices::Ptr inliers_plane) + void removePlaneSurface(const pcl::PointCloud::Ptr& cloud, + const pcl::PointIndices::Ptr& inliers_plane) { // create a SAC segmenter without using normals pcl::SACSegmentation segmentor; @@ -233,8 +237,9 @@ class CylinderSegment @param cloud - Pointcloud whose plane is removed. @param coefficients_cylinder - Cylinder parameters used to define an infinite cylinder will be stored here. @param cloud_normals - Point normals corresponding to the plane on which cylinder is kept */ - void extractCylinder(pcl::PointCloud::Ptr cloud, pcl::ModelCoefficients::Ptr coefficients_cylinder, - pcl::PointCloud::Ptr cloud_normals) + void extractCylinder(const pcl::PointCloud::Ptr& cloud, + const pcl::ModelCoefficients::Ptr& coefficients_cylinder, + const pcl::PointCloud::Ptr& cloud_normals) { // Create the segmentation object for cylinder segmentation and set all the parameters pcl::SACSegmentationFromNormals segmentor; @@ -288,8 +293,7 @@ class CylinderSegment // It will be used to extract the cylinder. extractNormals(cloud_normals, inliers_plane); // ModelCoefficients will hold the parameters using which we can define a cylinder of infinite length. - // It has a public attribute |code_start| values\ |code_end| of type |code_start| std::vector< float >\ |code_end|\ - // . + // It has a public attribute |code_start| values\ |code_end| of type |code_start| std::vector\ |code_end|\ . // |br| // |code_start| Values[0-2]\ |code_end| hold a point on the center line of the cylinder. |br| // |code_start| Values[3-5]\ |code_end| hold direction vector of the z-axis. |br| diff --git a/doc/planning_scene/src/planning_scene_tutorial.cpp b/doc/planning_scene/src/planning_scene_tutorial.cpp index 363d8bbc1..58b157670 100644 --- a/doc/planning_scene/src/planning_scene_tutorial.cpp +++ b/doc/planning_scene/src/planning_scene_tutorial.cpp @@ -49,7 +49,7 @@ // setStateFeasibilityPredicate function. Here's a simple example of a // user-defined callback that checks whether the "panda_joint1" of // the Panda robot is at a positive or negative angle: -bool stateFeasibilityTestExample(const moveit::core::RobotState& kinematic_state, bool verbose) +bool stateFeasibilityTestExample(const moveit::core::RobotState& kinematic_state, bool /*verbose*/) { const double* joint_values = kinematic_state.getJointPositions("panda_joint1"); return (joint_values[0] > 0.0); @@ -61,7 +61,7 @@ int main(int argc, char** argv) ros::init(argc, argv, "panda_arm_kinematics"); ros::AsyncSpinner spinner(1); spinner.start(); - std::size_t count = 0; + // BEGIN_TUTORIAL // // Setup @@ -78,7 +78,7 @@ int main(int argc, char** argv) // but this method of instantiation is only intended for illustration. robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); - moveit::core::RobotModelPtr kinematic_model = robot_model_loader.getModel(); + const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel(); planning_scene::PlanningScene planning_scene(kinematic_model); // Collision Checking diff --git a/doc/robot_model_and_robot_state/src/robot_model_and_robot_state_tutorial.cpp b/doc/robot_model_and_robot_state/src/robot_model_and_robot_state_tutorial.cpp index e8ec3dbc2..a5e010646 100644 --- a/doc/robot_model_and_robot_state/src/robot_model_and_robot_state_tutorial.cpp +++ b/doc/robot_model_and_robot_state/src/robot_model_and_robot_state_tutorial.cpp @@ -68,7 +68,7 @@ int main(int argc, char** argv) // .. _RobotModelLoader: // http://docs.ros.org/melodic/api/moveit_ros_planning/html/classrobot__model__loader_1_1RobotModelLoader.html robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); - moveit::core::RobotModelPtr kinematic_model = robot_model_loader.getModel(); + const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel(); ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str()); // Using the :moveit_core:`RobotModel`, we can construct a diff --git a/doc/state_display/src/state_display_tutorial.cpp b/doc/state_display/src/state_display_tutorial.cpp index f68623ff9..ff19a87a8 100644 --- a/doc/state_display/src/state_display_tutorial.cpp +++ b/doc/state_display/src/state_display_tutorial.cpp @@ -62,7 +62,7 @@ int main(int argc, char** argv) robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); /* Get a shared pointer to the model */ - moveit::core::RobotModelPtr kinematic_model = robot_model_loader.getModel(); + const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel(); /* Create a kinematic state - this represents the configuration for the robot represented by kinematic_model */ moveit::core::RobotStatePtr kinematic_state(new moveit::core::RobotState(kinematic_model)); diff --git a/doc/trajopt_planner/src/trajopt_example.cpp b/doc/trajopt_planner/src/trajopt_example.cpp index 73efabaff..f026a7646 100644 --- a/doc/trajopt_planner/src/trajopt_example.cpp +++ b/doc/trajopt_planner/src/trajopt_example.cpp @@ -227,13 +227,12 @@ int main(int argc, char** argv) moveit_msgs::MotionPlanResponse response; res.getMessage(response); - for (int timestep_index = 0; timestep_index < response.trajectory.joint_trajectory.points.size(); ++timestep_index) + for (size_t timestep_index = 0; timestep_index < response.trajectory.joint_trajectory.points.size(); ++timestep_index) { std::stringstream joint_values_stream; - for (int joint_index = 0; - joint_index < response.trajectory.joint_trajectory.points[timestep_index].positions.size(); ++joint_index) + for (double position : response.trajectory.joint_trajectory.points[timestep_index].positions) { - joint_values_stream << response.trajectory.joint_trajectory.points[timestep_index].positions[joint_index] << " "; + joint_values_stream << position << " "; } ROS_DEBUG_STREAM_NAMED(NODE_NAME, "step: " << timestep_index << " joints positions: " << joint_values_stream.str()); } diff --git a/doc/visualizing_collisions/src/visualizing_collisions_tutorial.cpp b/doc/visualizing_collisions/src/visualizing_collisions_tutorial.cpp index ac3f0790e..368f082ae 100644 --- a/doc/visualizing_collisions/src/visualizing_collisions_tutorial.cpp +++ b/doc/visualizing_collisions/src/visualizing_collisions_tutorial.cpp @@ -47,9 +47,9 @@ #include #include -planning_scene::PlanningScene* g_planning_scene = 0; +planning_scene::PlanningScene* g_planning_scene = nullptr; shapes::ShapePtr g_world_cube_shape; -ros::Publisher* g_marker_array_publisher = 0; +ros::Publisher* g_marker_array_publisher = nullptr; visualization_msgs::MarkerArray g_collision_points; void help() @@ -74,10 +74,10 @@ void help() void publishMarkers(visualization_msgs::MarkerArray& markers) { // delete old markers - if (g_collision_points.markers.size()) + if (!g_collision_points.markers.empty()) { - for (int i = 0; i < g_collision_points.markers.size(); i++) - g_collision_points.markers[i].action = visualization_msgs::Marker::DELETE; + for (auto& marker : g_collision_points.markers) + marker.action = visualization_msgs::Marker::DELETE; g_marker_array_publisher->publish(g_collision_points); } @@ -86,7 +86,7 @@ void publishMarkers(visualization_msgs::MarkerArray& markers) std::swap(g_collision_points.markers, markers.markers); // draw new markers (if there are any) - if (g_collision_points.markers.size()) + if (!g_collision_points.markers.empty()) g_marker_array_publisher->publish(g_collision_points); } diff --git a/htmlproofer.sh b/htmlproofer.sh new file mode 100755 index 000000000..2100ecc1e --- /dev/null +++ b/htmlproofer.sh @@ -0,0 +1,32 @@ +#!/bin/bash +set -e + +# Define some config vars +export NOKOGIRI_USE_SYSTEM_LIBRARIES=true +export REPOSITORY_NAME=${PWD##*/} +echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME" +sudo -E sh -c 'echo "deb http://packages.ros.org/ros/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list' +wget http://packages.ros.org/ros.key -O - | sudo apt-key add - +sudo apt-get update -qq +sudo apt-get install -qq -y python-rosdep python-wstool python-catkin-tools +# Setup rosdep +sudo rosdep init +rosdep update +# Install htmlpoofer +gem update --system +gem --version +gem install html-proofer +# Install ROS's version of sphinx +sudo apt-get -qq install ros-kinetic-rosdoc-lite +source /opt/ros/kinetic/setup.bash + +# Test build with non-ROS wrapped Sphinx command to allow warnings and errors to be caught +sphinx-build -W -b html . native_build +# Test build with ROS-version of Sphinx command so that it is generated same as ros.org +rosdoc_lite -o build . +# Run HTML tests on generated build output to check for 404 errors, etc +test "$TRAVIS_PULL_REQUEST" == false || URL_SWAP="--url-swap https\://github.com/ros-planning/moveit_tutorials/blob/master/:file\://$PWD/build/html/" +htmlproofer ./build --only-4xx --check-html --file-ignore ./build/html/genindex.html,./build/html/search.html --alt-ignore '/.*/' --url-ignore '#' $URL_SWAP + +# Tell GitHub Pages (on deploy) to bypass Jekyll processing +touch build/html/.nojekyll diff --git a/package.xml b/package.xml index d7dc10f74..0973dbfcc 100644 --- a/package.xml +++ b/package.xml @@ -36,6 +36,7 @@ tf2_geometry_msgs panda_moveit_config + franka_description pluginlib moveit_core moveit_commander @@ -45,13 +46,17 @@ interactive_markers moveit_visual_tools rviz_visual_tools + joint_state_publisher + robot_state_publisher joy pcl_ros pcl_conversions rosbag + rviz tf2_ros tf2_eigen tf2_geometry_msgs + xacro moveit_resources_panda_moveit_config rosunit