diff --git a/.travis.yml b/.travis.yml index da2511549..6a09bc37c 100644 --- a/.travis.yml +++ b/.travis.yml @@ -9,11 +9,16 @@ python: cache: ccache compiler: gcc +env: + global: + - ROS_DISTRO=melodic + - DOCKER_IMAGE=moveit/moveit:master-source + 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 ROS_DISTRO=melodic - - export DOCKER_IMAGE=moveit/moveit:master-source - - export NOKOGIRI_USE_SYSTEM_LIBRARIES=true - export REPOSITORY_NAME=${PWD##*/} - echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME" 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 aecf7eacd..a0d544864 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,11 +59,8 @@ class LERPInterface int dof_; private: - void interpolate(const std::vector joint_names, - robot_state::RobotStatePtr& robot_state, - const robot_state::JointModelGroup* joint_model_group, - const std::vector& start_joint_vals, - const std::vector& goal_joint_vals, - trajectory_msgs::JointTrajectory& joint_trajectory); + void interpolate(const std::vector joint_names, robot_state::RobotStatePtr& robot_state, + const robot_state::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 9dc885215..c575bc730 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 @@ -76,28 +76,28 @@ int main(int argc, char** argv) psm->startSceneMonitor(); psm->startWorldGeometryMonitor(); psm->startStateMonitor(); - + robot_model::RobotModelPtr robot_model = robot_model_loader->getModel(); - // Create a RobotState and to keep track of the current robot pose and planning group + // Create a RobotState and to keep track of the current robot pose and planning group robot_state::RobotStatePtr robot_state( new robot_state::RobotState(planning_scene_monitor::LockedPlanningSceneRO(psm)->getCurrentState())); robot_state->setToDefaultValues(); robot_state->update(); - // Create JointModelGroup + // Create JointModelGroup const robot_state::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()); - // Set the planner + // Set the planner std::string planner_plugin_name = "lerp_interface/LERPPlanner"; node_handle.setParam("planning_plugin", planner_plugin_name); - // Create pipeline + // Create pipeline planning_pipeline::PlanningPipelinePtr planning_pipeline( - new planning_pipeline::PlanningPipeline(robot_model, node_handle, "planning_plugin", "request_adapters")); + new planning_pipeline::PlanningPipeline(robot_model, node_handle, "planning_plugin", "request_adapters")); // ================================ Set the start and goal joint state planning_interface::MotionPlanRequest req; @@ -113,20 +113,22 @@ int main(int argc, char** argv) std::vector goal_joint_values = { 0.8, 0.7, 1, 1.3, 1.9, 2.2, 3 }; robot_state->setJointGroupPositions(joint_model_group, goal_joint_values); robot_state->update(); - moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(*robot_state, joint_model_group); + moveit_msgs::Constraints joint_goal = + kinematic_constraints::constructGoalConstraints(*robot_state, joint_model_group); req.goal_constraints.clear(); req.goal_constraints.push_back(joint_goal); req.goal_constraints[0].name = "goal_pos"; - // Set joint tolerance + // Set joint tolerance std::vector goal_joint_constraint = req.goal_constraints[0].joint_constraints; for (std::size_t x = 0; x < goal_joint_constraint.size(); ++x) { - ROS_INFO_STREAM_NAMED(NODE_NAME ," ======================================= joint position at goal: " << goal_joint_constraint[x].position); + ROS_INFO_STREAM_NAMED(NODE_NAME, " ======================================= joint position at goal: " + << goal_joint_constraint[x].position); req.goal_constraints[0].joint_constraints[x].tolerance_above = 0.001; req.goal_constraints[0].joint_constraints[x].tolerance_below = 0.001; } - + // ================================ Visualization tools namespace rvt = rviz_visual_tools; moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC, psm); @@ -147,7 +149,6 @@ int main(int argc, char** argv) /* We can also use visual_tools to wait for user input */ visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); - // ================================ planning context { planning_scene_monitor::LockedPlanningSceneRO lscene(psm); @@ -162,12 +163,11 @@ int main(int argc, char** argv) } visual_tools.prompt("Press 'next' to visualzie the result"); - // ================================ Visualize the trajectory // visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN); // visual_tools.trigger(); - + ros::Publisher display_publisher = node_handle.advertise("/move_group/display_planned_path", 1, true); moveit_msgs::DisplayTrajectory display_trajectory; 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 0b44ba160..6237bbc6d 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 @@ -62,7 +62,6 @@ bool LERPInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_ // Load the planner-specific parameters nh_.getParam("num_steps", num_steps_); - ros::WallTime start_time = ros::WallTime::now(); robot_model::RobotModelConstPtr robot_model = planning_scene->getRobotModel(); robot_state::RobotStatePtr start_state(new robot_state::RobotState(robot_model)); @@ -103,15 +102,13 @@ bool LERPInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_ return true; } -void LERPInterface::interpolate(const std::vector joint_names, - robot_state::RobotStatePtr& rob_state, - const robot_state::JointModelGroup* joint_model_group, - const std::vector& start_joint_vals, - const std::vector& goal_joint_vals, - trajectory_msgs::JointTrajectory& joint_trajectory) +void LERPInterface::interpolate(const std::vector joint_names, robot_state::RobotStatePtr& rob_state, + const robot_state::JointModelGroup* joint_model_group, + const std::vector& start_joint_vals, const std::vector& goal_joint_vals, + trajectory_msgs::JointTrajectory& joint_trajectory) { joint_trajectory.points.resize(num_steps_ + 1); - + std::vector dt_vector; for (int joint_index = 0; joint_index < dof_; ++joint_index) { @@ -134,5 +131,5 @@ void LERPInterface::interpolate(const std::vector joint_names, joint_trajectory.points[step].positions = joint_values; } } - + } // namespace lerp_interface diff --git a/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_planning_context.cpp b/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_planning_context.cpp index 509ff2665..a0e5d8315 100644 --- a/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_planning_context.cpp +++ b/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_planning_context.cpp @@ -97,7 +97,7 @@ bool LERPPlanningContext::terminate() { return true; } - + void LERPPlanningContext::clear() { // This planner has no state, so has nothing to clear diff --git a/doc/trajopt_planner/src/trajopt_example.cpp b/doc/trajopt_planner/src/trajopt_example.cpp index e36dc0a3e..9ae2e2e40 100644 --- a/doc/trajopt_planner/src/trajopt_example.cpp +++ b/doc/trajopt_planner/src/trajopt_example.cpp @@ -67,16 +67,17 @@ int main(int argc, char** argv) // Create pipeline planning_pipeline::PlanningPipelinePtr planning_pipeline( - new planning_pipeline::PlanningPipeline(robot_model, node_handle, "planning_plugin", "request_adapters")); + new planning_pipeline::PlanningPipeline(robot_model, node_handle, "planning_plugin", "request_adapters")); // Current state - std::vector current_joint_values = { 0, 0, 0, -1.5, 0, 0.6, 0.9}; + std::vector current_joint_values = { 0, 0, 0, -1.5, 0, 0.6, 0.9 }; robot_state->setJointGroupPositions(joint_model_group, current_joint_values); - + geometry_msgs::Pose pose_msg_current; - const Eigen::Isometry3d& end_effector_transform_current = robot_state->getGlobalLinkTransform(link_model_names.back()); + const Eigen::Isometry3d& end_effector_transform_current = + robot_state->getGlobalLinkTransform(link_model_names.back()); pose_msg_current = tf2::toMsg(end_effector_transform_current); - + // Create response and request planning_interface::MotionPlanRequest req; planning_interface::MotionPlanResponse res; @@ -104,20 +105,22 @@ int main(int argc, char** argv) geometry_msgs::Pose pose_msg_start; const Eigen::Isometry3d& end_effector_transform_start = robot_state->getGlobalLinkTransform(link_model_names.back()); pose_msg_start = tf2::toMsg(end_effector_transform_start); - - // Set the goal state + + // Set the goal state // ======================================================================================== std::vector goal_joint_values = { 0.8, 0.7, 1, -1.3, 1.9, 2.2, -0.1 }; robot_state->setJointGroupPositions(joint_model_group, goal_joint_values); robot_state->update(); - moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(*robot_state, joint_model_group); + moveit_msgs::Constraints joint_goal = + kinematic_constraints::constructGoalConstraints(*robot_state, joint_model_group); req.goal_constraints.push_back(joint_goal); req.goal_constraints[0].name = "goal_pos"; // Set joint tolerance std::vector goal_joint_constraint = req.goal_constraints[0].joint_constraints; for (std::size_t x = 0; x < goal_joint_constraint.size(); ++x) { - ROS_INFO_STREAM_NAMED(NODE_NAME ," ======================================= joint position at goal: " << goal_joint_constraint[x].position); + ROS_INFO_STREAM_NAMED(NODE_NAME, " ======================================= joint position at goal: " + << goal_joint_constraint[x].position); req.goal_constraints[0].joint_constraints[x].tolerance_above = 0.001; req.goal_constraints[0].joint_constraints[x].tolerance_below = 0.001; } @@ -142,10 +145,10 @@ int main(int argc, char** argv) // type: GIVEN_TRAJ // For this example, we give an interpolated trajectory - int const N_STEPS = 20; // number of steps - int const N_DOF = 7; // number of degrees of freedom + int const N_STEPS = 20; // number of steps + int const N_DOF = 7; // number of degrees of freedom - // Calculate the increment value for each joint + // Calculate the increment value for each joint std::vector dt_vector; for (int joint_index = 0; joint_index < N_DOF; ++joint_index) { @@ -156,7 +159,7 @@ int main(int argc, char** argv) req.reference_trajectories.resize(1); req.reference_trajectories[0].joint_trajectory.resize(1); // trajectory includes both the start and end points (N_STEPS + 1) - req.reference_trajectories[0].joint_trajectory[0].points.resize(N_STEPS + 1); + req.reference_trajectories[0].joint_trajectory[0].points.resize(N_STEPS + 1); req.reference_trajectories[0].joint_trajectory[0].joint_names = joint_names; req.reference_trajectories[0].joint_trajectory[0].points[0].positions = current_joint_values; // Use the increment values (dt_vector) to caluclate the joint values at each step @@ -232,9 +235,9 @@ int main(int argc, char** argv) { joint_values_stream << response.trajectory.joint_trajectory.points[timestep_index].positions[joint_index] << " "; } - ROS_DEBUG_STREAM_NAMED(NODE_NAME, "step: " << timestep_index << " joints positions: " << joint_values_stream.str() ); + ROS_DEBUG_STREAM_NAMED(NODE_NAME, "step: " << timestep_index << " joints positions: " << joint_values_stream.str()); } - + display_trajectory.trajectory_start = response.trajectory_start; display_trajectory.trajectory.push_back(response.trajectory);