Skip to content

Commit

Permalink
Apply clang-format 3.8 changes
Browse files Browse the repository at this point in the history
  • Loading branch information
davetcoleman committed Nov 18, 2016
1 parent 736b52d commit d6e7618
Show file tree
Hide file tree
Showing 43 changed files with 134 additions and 104 deletions.
20 changes: 19 additions & 1 deletion .clang-format
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,6 @@ Standard: Auto
IndentWidth: 2
TabWidth: 2
UseTab: Never
BreakBeforeBraces: Allman
IndentFunctionDeclarationAfterType: false
SpacesInParentheses: false
SpacesInAngles: false
Expand All @@ -45,4 +44,23 @@ SpacesInCStyleCastParentheses: 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'
}
...
11 changes: 3 additions & 8 deletions moveit_core/constraint_samplers/test/pr2_arm_ik.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -257,8 +257,7 @@ void PR2ArmIK::computeIKShoulderPan(const Eigen::Matrix4f& g_in, const double& t
theta4[1] = -acos_angle;

#ifdef DEBUG
std::cout << "ComputeIK::theta3:" << numerator << "," << denominator << "," << std::endl
<< theta4[0] << std::endl;
std::cout << "ComputeIK::theta3:" << numerator << "," << denominator << "," << std::endl << theta4[0] << std::endl;
#endif

for (int jj = 0; jj < 2; jj++)
Expand Down Expand Up @@ -420,9 +419,7 @@ void PR2ArmIK::computeIKShoulderPan(const Eigen::Matrix4f& g_in, const double& t
std::cout << "theta4: " << t4 << std::endl;
std::cout << "theta5: " << t5 << std::endl;
std::cout << "theta6: " << t6 << std::endl;
std::cout << "theta7: " << t7 << std::endl
<< std::endl
<< std::endl;
std::cout << "theta7: " << t7 << std::endl << std::endl << std::endl;
#endif
for (int lll = 0; lll < 2; lll++)
{
Expand Down Expand Up @@ -743,9 +740,7 @@ void PR2ArmIK::computeIKShoulderRoll(const Eigen::Matrix4f& g_in, const double&
std::cout << "theta4: " << t4 << std::endl;
std::cout << "theta5: " << t5 << std::endl;
std::cout << "theta6: " << t6 << std::endl;
std::cout << "theta7: " << t7 << std::endl
<< std::endl
<< std::endl;
std::cout << "theta7: " << t7 << std::endl << std::endl << std::endl;
#endif

solution_ik[0] = normalize_angle(t1 * angle_multipliers_[0]);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -348,8 +348,7 @@ VoxelGrid<T>::VoxelGrid(double size_x, double size_y, double size_z, double reso
}

template <typename T>
VoxelGrid<T>::VoxelGrid()
: data_(NULL)
VoxelGrid<T>::VoxelGrid() : data_(NULL)
{
for (int i = DIM_X; i <= DIM_Z; ++i)
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -148,7 +148,8 @@ class KinematicsBase

/** @brief The signature for a callback that can compute IK */
typedef boost::function<void(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_solution,
moveit_msgs::MoveItErrorCodes& error_code)> IKCallbackFn;
moveit_msgs::MoveItErrorCodes& error_code)>
IKCallbackFn;

/**
* @brief Given a desired pose of the end-effector, compute the joint angles to reach it
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,8 @@ class PlanningRequestAdapter
public:
typedef boost::function<bool(const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res)> PlannerFn;
planning_interface::MotionPlanResponse& res)>
PlannerFn;

PlanningRequestAdapter()
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,8 @@ typedef std::map<std::string, const LinkModel*> LinkModelMapConst;

/** \brief Map from link model instances to Eigen transforms */
typedef std::map<const LinkModel*, Eigen::Affine3d, std::less<const LinkModel*>,
Eigen::aligned_allocator<std::pair<const LinkModel*, Eigen::Affine3d> > > LinkTransformMap;
Eigen::aligned_allocator<std::pair<const LinkModel*, Eigen::Affine3d> > >
LinkTransformMap;

/** \brief A link from the robot. Contains the constant transform applied to the link and its geometry */
class LinkModel
Expand Down
3 changes: 2 additions & 1 deletion moveit_core/robot_model/src/robot_model.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -130,7 +130,8 @@ namespace core
namespace
{
typedef std::map<const JointModel*, std::pair<std::set<const LinkModel*, OrderLinksByIndex>,
std::set<const JointModel*, OrderJointsByIndex> > > DescMap;
std::set<const JointModel*, OrderJointsByIndex> > >
DescMap;

void computeDescendantsHelper(const JointModel* joint, std::vector<const JointModel*>& parents,
std::set<const JointModel*>& seen, DescMap& descendants)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,8 @@ MOVEIT_CLASS_FORWARD(RobotState);
the state is valid or not. Returns true if the state is valid. This call is allowed to modify \e robot_state (e.g.,
set \e joint_group_variable_values) */
typedef boost::function<bool(RobotState* robot_state, const JointModelGroup* joint_group,
const double* joint_group_variable_values)> GroupStateValidityCallbackFn;
const double* joint_group_variable_values)>
GroupStateValidityCallbackFn;

/** \brief Representation of a robot's state. This includes position,
velocity, acceleration and effort.
Expand Down Expand Up @@ -965,8 +966,8 @@ as the new values that correspond to the group */
Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false)
{
updateLinkTransforms();
return const_cast<const RobotState*>(this)
->getJacobian(group, link, reference_point_position, jacobian, use_quaternion_representation);
return const_cast<const RobotState*>(this)->getJacobian(group, link, reference_point_position, jacobian,
use_quaternion_representation);
}

/** \brief Compute the Jacobian with reference to the last link of a specified group. If the group is not a chain, an
Expand Down
3 changes: 2 additions & 1 deletion moveit_core/robot_state/src/robot_state.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -599,7 +599,8 @@ void moveit::core::RobotState::updateStateWithLinkAt(const LinkModel* link, cons
global_link_transforms_[parent_link->getLinkIndex()] =
global_link_transforms_[child_link->getLinkIndex()] *
(child_link->getJointOriginTransform() *
variable_joint_transforms_[child_link->getParentJointModel()->getJointIndex()]).inverse();
variable_joint_transforms_[child_link->getParentJointModel()->getJointIndex()])
.inverse();

// update link transforms for descendant links only (leaving the transform for the current link untouched)
// with the exception of the child link we are coming backwards from
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,8 @@ MOVEIT_CLASS_FORWARD(Transforms);
/// @brief Map frame names to the transformation matrix that can transform objects from the frame name to the planning
/// frame
typedef std::map<std::string, Eigen::Affine3d, std::less<std::string>,
Eigen::aligned_allocator<std::pair<const std::string, Eigen::Affine3d> > > FixedTransformsMap;
Eigen::aligned_allocator<std::pair<const std::string, Eigen::Affine3d> > >
FixedTransformsMap;

/** @brief Provides an implementation of a snapshot of a transform tree that can be easily queried for
transforming different quantities. Transforms are maintained as a list of transforms to a particular frame.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -283,7 +283,7 @@ int ChainIkSolverVel_pinv_mimic::CartToJnt(const JntArray& q_in, const Twist& v_
for (j = 0; j < rows; j++)
{
if (!position_ik)
sum += U[j](i)*v_in(j);
sum += U[j](i) * v_in(j);
else
sum += U_translate(j, i) * v_in(j);
}
Expand All @@ -302,7 +302,7 @@ int ChainIkSolverVel_pinv_mimic::CartToJnt(const JntArray& q_in, const Twist& v_
for (j = 0; j < jac_reduced.columns(); j++)
{
if (!position_ik)
sum += V[i](j)*tmp(j);
sum += V[i](j) * tmp(j);
else
sum += V_translate(i, j) * tmp(j);
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -294,7 +294,7 @@ int ChainIkSolverVel_pinv_mimic::CartToJnt(const JntArray& q_in, const Twist& v_
for (j = 0; j < rows; j++)
{
if (!position_ik)
sum += U[j](i)*v_in(j);
sum += U[j](i) * v_in(j);
else
sum += U_translate(j, i) * v_in(j);
}
Expand All @@ -313,7 +313,7 @@ int ChainIkSolverVel_pinv_mimic::CartToJnt(const JntArray& q_in, const Twist& v_
for (j = 0; j < jac_reduced.columns(); j++)
{
if (!position_ik)
sum += V[i](j)*tmp(j);
sum += V[i](j) * tmp(j);
else
sum += V_translate(i, j) * tmp(j);
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -86,8 +86,7 @@ bool SrvKinematicsPlugin::initialize(const std::string& robot_description, const

if (debug)
{
std::cout << std::endl
<< "Joint Model Variable Names: ------------------------------------------- " << std::endl;
std::cout << std::endl << "Joint Model Variable Names: ------------------------------------------- " << std::endl;
const std::vector<std::string> jm_names = joint_model_group_->getVariableNames();
std::copy(jm_names.begin(), jm_names.end(), std::ostream_iterator<std::string>(std::cout, "\n"));
std::cout << std::endl;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,9 @@ MOVEIT_CLASS_FORWARD(ConstraintsLibrary);

struct ModelBasedPlanningContextSpecification;
typedef boost::function<ob::PlannerPtr(const ompl::base::SpaceInformationPtr& si, const std::string& name,
const ModelBasedPlanningContextSpecification& spec)> ConfiguredPlannerAllocator;
const ModelBasedPlanningContextSpecification& spec)>
ConfiguredPlannerAllocator;

typedef boost::function<ConfiguredPlannerAllocator(const std::string& planner_type)> ConfiguredPlannerSelector;

struct ModelBasedPlanningContextSpecification
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,8 @@
namespace ompl_interface
{
typedef boost::function<bool(const ompl::base::State* from, const ompl::base::State* to, const double t,
ompl::base::State* state)> InterpolationFunction;
ompl::base::State* state)>
InterpolationFunction;
typedef boost::function<double(const ompl::base::State* state1, const ompl::base::State* state2)> DistanceFunction;

struct ModelBasedStateSpaceSpecification
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -86,7 +86,7 @@ ompl::base::State* ompl_interface::PoseModelStateSpace::allocState() const
StateType* state = new StateType();
state->values =
new double[variable_count_]; // need to allocate this here since ModelBasedStateSpace::allocState() is not called
state->poses = new ompl::base::SE3StateSpace::StateType* [poses_.size()];
state->poses = new ompl::base::SE3StateSpace::StateType*[poses_.size()];
for (std::size_t i = 0; i < poses_.size(); ++i)
state->poses[i] = poses_[i].state_space_->allocState()->as<ompl::base::SE3StateSpace::StateType>();
return state;
Expand Down
6 changes: 4 additions & 2 deletions moveit_planners/ompl/ompl_interface/test/test_state_space.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -157,11 +157,13 @@ TEST_F(LoadPlanningModelsPr2, StateSpaceCopy)
ss.copyToOMPLState(state, kstate);
kstate.setToRandomPositions(kstate.getRobotModel()->getJointModelGroup(ss.getJointModelGroupName()));
std::cout << (kstate.getGlobalLinkTransform("r_wrist_roll_link").translation() -
kstate2.getGlobalLinkTransform("r_wrist_roll_link").translation()) << std::endl;
kstate2.getGlobalLinkTransform("r_wrist_roll_link").translation())
<< std::endl;
EXPECT_TRUE(kstate.distance(kstate2) > 1e-12);
ss.copyToRobotState(kstate, state);
std::cout << (kstate.getGlobalLinkTransform("r_wrist_roll_link").translation() -
kstate2.getGlobalLinkTransform("r_wrist_roll_link").translation()) << std::endl;
kstate2.getGlobalLinkTransform("r_wrist_roll_link").translation())
<< std::endl;
EXPECT_TRUE(kstate.distance(kstate2) < 1e-12);
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -237,7 +237,7 @@ class EnvironmentChain3D : public DiscreteSpaceInformation
void attemptShortcut(const trajectory_msgs::JointTrajectory& traj_in, trajectory_msgs::JointTrajectory& traj_out);

protected:
bool getGridXYZInt(const Eigen::Affine3d& pose, int(&xyz)[3]) const;
bool getGridXYZInt(const Eigen::Affine3d& pose, int (&xyz)[3]) const;

void getMotionPrimitives(const std::string& group);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -115,7 +115,7 @@ struct EnvChain3DPlanningData
}

EnvChain3DHashEntry* addHashEntry(const std::vector<int>& coord, const std::vector<double>& angles,
const int(&xyz)[3], int action)
const int (&xyz)[3], int action)
{
EnvChain3DHashEntry* new_hash_entry = new EnvChain3DHashEntry();
new_hash_entry->stateID = state_ID_to_coord_table_.size();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -886,7 +886,7 @@ int EnvironmentChain3D::getEndEffectorHeuristic(int from_stateID, int to_stateID
//}
}

bool EnvironmentChain3D::getGridXYZInt(const Eigen::Affine3d& pose, int(&xyz)[3]) const
bool EnvironmentChain3D::getGridXYZInt(const Eigen::Affine3d& pose, int (&xyz)[3]) const
{
if (!gsr_ || !gsr_->dfce_->distance_field_)
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -77,12 +77,15 @@ bool SBPLInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_
std::cerr << "B ret is " << b_ret << " planning time " << el << std::endl;
std::cerr << "Expansions " << env_chain->getPlanningStatistics().total_expansions_ << " average time "
<< (env_chain->getPlanningStatistics().total_expansion_time_.toSec() /
(env_chain->getPlanningStatistics().total_expansions_ * 1.0)) << " hz "
(env_chain->getPlanningStatistics().total_expansions_ * 1.0))
<< " hz "
<< 1.0 / (env_chain->getPlanningStatistics().total_expansion_time_.toSec() /
(env_chain->getPlanningStatistics().total_expansions_ * 1.0)) << std::endl;
(env_chain->getPlanningStatistics().total_expansions_ * 1.0))
<< std::endl;
std::cerr << "Total coll checks " << env_chain->getPlanningStatistics().coll_checks_ << " hz "
<< 1.0 / (env_chain->getPlanningStatistics().total_coll_check_time_.toSec() /
(env_chain->getPlanningStatistics().coll_checks_ * 1.0)) << std::endl;
(env_chain->getPlanningStatistics().coll_checks_ * 1.0))
<< std::endl;
std::cerr << "Path length is " << solution_state_ids.size() << std::endl;
if (!b_ret)
{
Expand Down
13 changes: 4 additions & 9 deletions moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1170,7 +1170,8 @@ void moveit_benchmarks::BenchmarkExecution::runPlanningBenchmark(BenchmarkReques
{
// Output name of planning algorithm
out << planner_interfaces_to_benchmark[q]->getDescription() + "_" +
planner_ids_to_benchmark_per_planner_interface[q][p] << std::endl;
planner_ids_to_benchmark_per_planner_interface[q][p]
<< std::endl;

// in general, we could have properties specific for a planner;
// right now, we do not include such properties
Expand Down Expand Up @@ -1312,10 +1313,7 @@ void moveit_benchmarks::BenchmarkExecution::runGoalExistenceBenchmark(BenchmarkR
out << "Experiment " << (planning_scene_->getName().empty() ? "NO_NAME" : planning_scene_->getName()) << std::endl;
out << "Running on " << (host.empty() ? "UNKNOWN" : host) << std::endl;
out << "Starting at " << boost::posix_time::to_iso_extended_string(startTime.toBoost()) << std::endl;
out << "<<<|" << std::endl
<< "ROS" << std::endl
<< req.motion_plan_request << std::endl
<< "|>>>" << std::endl;
out << "<<<|" << std::endl << "ROS" << std::endl << req.motion_plan_request << std::endl << "|>>>" << std::endl;
out << req.motion_plan_request.allowed_planning_time << " seconds per run" << std::endl;
out << duration << " seconds spent to collect the data" << std::endl;
out << "reachable BOOLEAN" << std::endl;
Expand All @@ -1340,10 +1338,7 @@ void moveit_benchmarks::BenchmarkExecution::runGoalExistenceBenchmark(BenchmarkR
out << "Experiment " << (planning_scene_->getName().empty() ? "NO_NAME" : planning_scene_->getName()) << std::endl;
out << "Running on " << (host.empty() ? "UNKNOWN" : host) << std::endl;
out << "Starting at " << boost::posix_time::to_iso_extended_string(startTime.toBoost()) << std::endl;
out << "<<<|" << std::endl
<< "ROS" << std::endl
<< req.motion_plan_request << std::endl
<< "|>>>" << std::endl;
out << "<<<|" << std::endl << "ROS" << std::endl << req.motion_plan_request << std::endl << "|>>>" << std::endl;
out << req.motion_plan_request.allowed_planning_time << " seconds per run" << std::endl;
out << "reachable BOOLEAN" << std::endl;
out << "collision_free BOOLEAN" << std::endl;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,8 @@ class BenchmarkExecutor
/// Definition of a post-run benchmark event function. Invoked immediately after each planner calls solve().
typedef boost::function<void(const moveit_msgs::MotionPlanRequest& request,
const planning_interface::MotionPlanDetailedResponse& response,
PlannerRunData& run_data)> PostRunEventFunction;
PlannerRunData& run_data)>
PostRunEventFunction;

BenchmarkExecutor(const std::string& robot_description_param = "robot_description");
virtual ~BenchmarkExecutor();
Expand Down
7 changes: 2 additions & 5 deletions moveit_ros/benchmarks/src/BenchmarkExecutor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -941,11 +941,8 @@ void BenchmarkExecutor::writeOutput(const BenchmarkRequest& brequest, const std:
moveit_msgs::PlanningScene scene_msg;
planning_scene_->getPlanningSceneMsg(scene_msg);
out << "<<<|" << std::endl;
out << "Motion plan request:" << std::endl
<< brequest.request << std::endl;
out << "Planning scene: " << std::endl
<< scene_msg << std::endl
<< "|>>>" << std::endl;
out << "Motion plan request:" << std::endl << brequest.request << std::endl;
out << "Planning scene: " << std::endl << scene_msg << std::endl << "|>>>" << std::endl;

// Not writing optional cpu information

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -198,9 +198,8 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const

// if translation vectors are specified in the frame of the ik link name, then we assume the frame is local;
// otherwise, the frame is global
bool approach_direction_is_global_frame =
!robot_state::Transforms::sameFrame(plan->approach_.direction.header.frame_id,
plan->shared_data_->ik_link_->getName());
bool approach_direction_is_global_frame = !robot_state::Transforms::sameFrame(
plan->approach_.direction.header.frame_id, plan->shared_data_->ik_link_->getName());
bool retreat_direction_is_global_frame = !robot_state::Transforms::sameFrame(plan->retreat_.direction.header.frame_id,
plan->shared_data_->ik_link_->getName());

Expand Down
6 changes: 3 additions & 3 deletions moveit_ros/perception/mesh_filter/src/mesh_filter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -256,7 +256,7 @@ void mesh_filter::MeshFilter::getFilteredDepth(float* depth) const
const float offset = depth_filter_.getNearClippingDistance();
while (depth < depthEnd && idx++ < first)
if (*depth != 0 && *depth != 1.0)
*depth = *depth* scale + offset;
*depth = *depth * scale + offset;
else
*depth = 0;

Expand All @@ -265,7 +265,7 @@ void mesh_filter::MeshFilter::getFilteredDepth(float* depth) const
float* depth2 = depthEnd - last;
while (depth2 < depthEnd)
if (*depth2 != 0 && *depth != 1.0)
*depth2 = *depth2* scale + offset;
*depth2 = *depth2 * scale + offset;
else
*depth2 = 0;

Expand All @@ -291,7 +291,7 @@ void mesh_filter::MeshFilter::getFilteredDepth(float* depth) const
// 0 = on near clipping plane -> we used 0 to mark invalid points -> not visible
// points on far clipping plane needs to be removed too
if (*depth != 0 && *depth != 1.0)
*depth = *depth* scale + offset;
*depth = *depth * scale + offset;
else
*depth = 0;

Expand Down

0 comments on commit d6e7618

Please sign in to comment.