Skip to content

Commit

Permalink
Travis: enable clang-format (moveit#458)
Browse files Browse the repository at this point in the history 
* enable clang-format in Travis
* apply clang-format
  • Loading branch information
@jschleicher
jschleicher authored Feb 21, 2020
1 parent 7882a96 commit 9acbbe0
Show file tree
Hide file tree
Showing 6 changed files with 49 additions and 47 deletions.
11 changes: 8 additions & 3 deletions .travis.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
9 changes: 3 additions & 6 deletions doc/creating_moveit_plugins/lerp_motion_planner/include/lerp_interface/lerp_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,11 +59,8 @@ class LERPInterface
int dof_;

private:
void interpolate(const std::vector<std::string> joint_names,
robot_state::RobotStatePtr& robot_state,
const robot_state::JointModelGroup* joint_model_group,
const std::vector<double>& start_joint_vals,
const std::vector<double>& goal_joint_vals,
trajectory_msgs::JointTrajectory& joint_trajectory);
void interpolate(const std::vector<std::string> joint_names, robot_state::RobotStatePtr& robot_state,
const robot_state::JointModelGroup* joint_model_group, const std::vector<double>& start_joint_vals,
const std::vector<double>& goal_joint_vals, trajectory_msgs::JointTrajectory& joint_trajectory);
};
}
26 changes: 13 additions & 13 deletions doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string>& joint_names = joint_model_group->getActiveJointModelNames();
const std::vector<std::string>& 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;
Expand All @@ -113,20 +113,22 @@ int main(int argc, char** argv)
std::vector<double> 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<moveit_msgs::JointConstraint> 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);
Expand All @@ -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);
Expand All @@ -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<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
moveit_msgs::DisplayTrajectory display_trajectory;
Expand Down
15 changes: 6 additions & 9 deletions doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down Expand Up @@ -103,15 +102,13 @@ bool LERPInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_
return true;
}

void LERPInterface::interpolate(const std::vector<std::string> joint_names,
robot_state::RobotStatePtr& rob_state,
const robot_state::JointModelGroup* joint_model_group,
const std::vector<double>& start_joint_vals,
const std::vector<double>& goal_joint_vals,
trajectory_msgs::JointTrajectory& joint_trajectory)
void LERPInterface::interpolate(const std::vector<std::string> joint_names, robot_state::RobotStatePtr& rob_state,
const robot_state::JointModelGroup* joint_model_group,
const std::vector<double>& start_joint_vals, const std::vector<double>& goal_joint_vals,
trajectory_msgs::JointTrajectory& joint_trajectory)
{
joint_trajectory.points.resize(num_steps_ + 1);

std::vector<double> dt_vector;
for (int joint_index = 0; joint_index < dof_; ++joint_index)
{
Expand All @@ -134,5 +131,5 @@ void LERPInterface::interpolate(const std::vector<std::string> joint_names,
joint_trajectory.points[step].positions = joint_values;
}
}

} // namespace lerp_interface
2 changes: 1 addition & 1 deletion doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_planning_context.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,7 +97,7 @@ bool LERPPlanningContext::terminate()
{
return true;
}

void LERPPlanningContext::clear()
{
// This planner has no state, so has nothing to clear
Expand Down
33 changes: 18 additions & 15 deletions doc/trajopt_planner/src/trajopt_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<double> current_joint_values = { 0, 0, 0, -1.5, 0, 0.6, 0.9};
std::vector<double> 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;
Expand Down Expand Up @@ -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<double> 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<moveit_msgs::JointConstraint> 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;
}
Expand All @@ -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<double> dt_vector;
for (int joint_index = 0; joint_index < N_DOF; ++joint_index)
{
Expand All @@ -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
Expand Down Expand Up @@ -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);

Expand Down

0 comments on commit 9acbbe0

Please sign in to comment.