From ee6c50ad31402b00fc4b8a0d8fcd62b1c85c27b7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Thu, 20 Aug 2020 11:56:53 +0200 Subject: [PATCH] Apply clang-format-10 (#199) --- .clang-format | 1 + .../moveit/task_constructor/container.h | 2 +- .../moveit/task_constructor/container_p.h | 4 +- .../moveit/task_constructor/marker_tools.h | 4 +- .../solvers/planner_interface.h | 4 +- core/include/moveit/task_constructor/stage.h | 2 +- .../task_constructor/stages/compute_ik.h | 4 +- .../moveit/task_constructor/stages/connect.h | 2 +- .../stages/modify_planning_scene.h | 2 +- .../task_constructor/stages/move_relative.h | 2 +- .../moveit/task_constructor/stages/move_to.h | 6 +-- .../stages/predicate_filter.h | 2 +- .../task_constructor/stages/simple_grasp.h | 2 +- .../include/moveit/task_constructor/storage.h | 8 +-- core/include/moveit/task_constructor/task.h | 6 +-- .../moveit/task_constructor/type_traits.h | 5 +- core/src/introspection.cpp | 2 +- core/src/solvers/planner_interface.cpp | 4 +- core/src/stage.cpp | 2 +- core/src/stages/compute_ik.cpp | 5 +- core/src/stages/fix_collision_objects.cpp | 4 +- core/src/stages/move_to.cpp | 4 +- core/src/task.cpp | 6 +-- core/test/models.h | 2 +- .../properties/property_factory.h | 4 +- .../motion_planning_tasks/src/icons.cpp | 4 +- .../motion_planning_tasks/src/icons.h | 4 +- .../src/remote_task_model.cpp | 2 +- .../motion_planning_tasks/src/task_display.h | 4 +- .../src/task_list_model.h | 2 +- .../motion_planning_tasks/src/task_panel.cpp | 2 +- .../motion_planning_tasks/src/task_panel.h | 4 +- .../test/test_merge_models.cpp | 23 +++++---- .../visualization_tools/display_solution.h | 2 +- .../marker_visualization.h | 4 +- .../task_solution_visualization.h | 2 +- .../src/task_solution_visualization.cpp | 50 ++++++++++--------- 37 files changed, 99 insertions(+), 93 deletions(-) diff --git a/.clang-format b/.clang-format index 2594f05a0..c7ace03b4 100644 --- a/.clang-format +++ b/.clang-format @@ -24,6 +24,7 @@ BraceWrapping: BeforeCatch: false BeforeElse: false IndentBraces: false + SplitEmptyRecord: false BreakBeforeBinaryOperators: None BreakBeforeBraces: Custom BreakBeforeTernaryOperators: false diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index e7b241758..27b2fde0a 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -120,7 +120,7 @@ class ParallelContainerBase; * - Fallbacks: the children are considered in series * - Merger: solutions of all children (actuating disjoint groups) * are merged into a single solution for parallel execution -*/ + */ class ParallelContainerBase : public ContainerBase { public: diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 98e702869..61eae9a12 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -49,8 +49,8 @@ namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(JointModelGroup) MOVEIT_CLASS_FORWARD(RobotState) -} -} +} // namespace core +} // namespace moveit namespace moveit { namespace task_constructor { diff --git a/core/include/moveit/task_constructor/marker_tools.h b/core/include/moveit/task_constructor/marker_tools.h index 28da4ddbd..f4d762822 100644 --- a/core/include/moveit/task_constructor/marker_tools.h +++ b/core/include/moveit/task_constructor/marker_tools.h @@ -12,8 +12,8 @@ namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotState) MOVEIT_CLASS_FORWARD(LinkModel) -} -} +} // namespace core +} // namespace moveit namespace moveit { namespace task_constructor { diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h index c0edad2bf..45a033e48 100644 --- a/core/include/moveit/task_constructor/solvers/planner_interface.h +++ b/core/include/moveit/task_constructor/solvers/planner_interface.h @@ -54,8 +54,8 @@ namespace core { MOVEIT_CLASS_FORWARD(LinkModel) MOVEIT_CLASS_FORWARD(RobotModel) MOVEIT_CLASS_FORWARD(JointModelGroup) -} -} +} // namespace core +} // namespace moveit namespace moveit { namespace task_constructor { diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 3392b3208..4b4fcd305 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -54,7 +54,7 @@ namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotModel) } -} +} // namespace moveit namespace planning_scene { MOVEIT_CLASS_FORWARD(PlanningScene) diff --git a/core/include/moveit/task_constructor/stages/compute_ik.h b/core/include/moveit/task_constructor/stages/compute_ik.h index d72446931..447c088a8 100644 --- a/core/include/moveit/task_constructor/stages/compute_ik.h +++ b/core/include/moveit/task_constructor/stages/compute_ik.h @@ -45,8 +45,8 @@ namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotState) MOVEIT_CLASS_FORWARD(JointModelGroup) -} -} +} // namespace core +} // namespace moveit namespace moveit { namespace task_constructor { diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index 6271b5e14..f2913c3f3 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -47,7 +47,7 @@ namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotState) } -} +} // namespace moveit namespace moveit { namespace task_constructor { diff --git a/core/include/moveit/task_constructor/stages/modify_planning_scene.h b/core/include/moveit/task_constructor/stages/modify_planning_scene.h index 9fd9c7c95..bcad42b1c 100644 --- a/core/include/moveit/task_constructor/stages/modify_planning_scene.h +++ b/core/include/moveit/task_constructor/stages/modify_planning_scene.h @@ -48,7 +48,7 @@ namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(JointModelGroup) } -} +} // namespace moveit namespace moveit { namespace task_constructor { diff --git a/core/include/moveit/task_constructor/stages/move_relative.h b/core/include/moveit/task_constructor/stages/move_relative.h index 821e73b0b..31890d677 100644 --- a/core/include/moveit/task_constructor/stages/move_relative.h +++ b/core/include/moveit/task_constructor/stages/move_relative.h @@ -48,7 +48,7 @@ namespace moveit { namespace core { class RobotState; } -} +} // namespace moveit namespace moveit { namespace task_constructor { namespace stages { diff --git a/core/include/moveit/task_constructor/stages/move_to.h b/core/include/moveit/task_constructor/stages/move_to.h index c4da6c44e..47d53c060 100644 --- a/core/include/moveit/task_constructor/stages/move_to.h +++ b/core/include/moveit/task_constructor/stages/move_to.h @@ -48,7 +48,7 @@ namespace moveit { namespace core { class RobotState; } -} +} // namespace moveit namespace moveit { namespace task_constructor { namespace stages { @@ -101,10 +101,10 @@ class MoveTo : public PropagatingEitherWay bool getJointStateGoal(const boost::any& goal, const core::JointModelGroup* jmg, moveit::core::RobotState& state); bool getPoseGoal(const boost::any& goal, const geometry_msgs::PoseStamped& ik_pose_msg, const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen, - decltype(std::declval().markers()) & markers); + decltype(std::declval().markers())& markers); bool getPointGoal(const boost::any& goal, const moveit::core::LinkModel* link, const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen, - decltype(std::declval().markers()) & /*unused*/); + decltype(std::declval().markers())& /*unused*/); protected: solvers::PlannerInterfacePtr planner_; diff --git a/core/include/moveit/task_constructor/stages/predicate_filter.h b/core/include/moveit/task_constructor/stages/predicate_filter.h index 6088c0f38..c2ce289c0 100644 --- a/core/include/moveit/task_constructor/stages/predicate_filter.h +++ b/core/include/moveit/task_constructor/stages/predicate_filter.h @@ -41,7 +41,7 @@ namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotState) } -} +} // namespace moveit namespace moveit { namespace task_constructor { diff --git a/core/include/moveit/task_constructor/stages/simple_grasp.h b/core/include/moveit/task_constructor/stages/simple_grasp.h index cdac84626..df81d2fe6 100644 --- a/core/include/moveit/task_constructor/stages/simple_grasp.h +++ b/core/include/moveit/task_constructor/stages/simple_grasp.h @@ -45,7 +45,7 @@ namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotModel) } -} +} // namespace moveit namespace moveit { namespace task_constructor { namespace stages { diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index efd3030cb..e0f7dd6f7 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -182,10 +182,10 @@ class Interface : public ordered // restrict access to some functions to ensure consistency // (we need to set/unset InterfaceState::owner_) - using base_type::moveTo; - using base_type::moveFrom; - using base_type::insert; using base_type::erase; + using base_type::insert; + using base_type::moveFrom; + using base_type::moveTo; using base_type::remove_if; }; @@ -329,4 +329,4 @@ struct less return *x < *y; } }; -} +} // namespace std diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 08bd5d62c..ce6e6c04f 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -52,8 +52,8 @@ namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotModel) MOVEIT_CLASS_FORWARD(RobotState) -} -} +} // namespace core +} // namespace moveit namespace moveit { namespace task_constructor { @@ -108,9 +108,9 @@ class Task : protected WrapperBase void eraseTaskCallback(TaskCallbackList::const_iterator which); /// expose SolutionCallback API - using WrapperBase::SolutionCallback; using WrapperBase::addSolutionCallback; using WrapperBase::removeSolutionCallback; + using WrapperBase::SolutionCallback; /// reset all stages void reset() final; diff --git a/core/include/moveit/task_constructor/type_traits.h b/core/include/moveit/task_constructor/type_traits.h index 876beb54d..2390e8f18 100644 --- a/core/include/moveit/task_constructor/type_traits.h +++ b/core/include/moveit/task_constructor/type_traits.h @@ -54,8 +54,9 @@ struct is_container_helper template struct is_container< - T, std::conditional_t().cbegin()), - decltype(std::declval().cend())>, + T, std::conditional_t().cbegin()), + decltype(std::declval().cend())>, void> > : public std::true_type {}; } // namespace task_constructor diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index 3edd2f92b..8873c68ae 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -57,7 +57,7 @@ std::string getProcessId() { gethostname(our_hostname, sizeof(our_hostname) - 1); return std::to_string(getpid()) + "@" + our_hostname; } -} +} // namespace class IntrospectionPrivate { diff --git a/core/src/solvers/planner_interface.cpp b/core/src/solvers/planner_interface.cpp index d120eecef..23e314733 100644 --- a/core/src/solvers/planner_interface.cpp +++ b/core/src/solvers/planner_interface.cpp @@ -47,6 +47,6 @@ PlannerInterface::PlannerInterface() { p.declare("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor"); p.declare("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor"); } -} -} +} // namespace solvers +} // namespace task_constructor } // namespace moveit diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 990c85680..3c4c94151 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -720,7 +720,7 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta auto it = std::find_if(to_attached.cbegin(), to_attached.cend(), [from_object](const moveit::core::AttachedBody* object) { return object->getName() == from_object->getName(); - }); + }); if (it == to_attached.cend()) { ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": object missing: " << from_object->getName()); return false; diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 3b885795e..a1b9d4c49 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -354,8 +354,9 @@ void ComputeIK::compute() { double min_solution_distance = props.get("min_solution_distance"); IKSolutions ik_solutions; - auto is_valid = [sandbox_scene, ignore_collisions, min_solution_distance, &ik_solutions]( - robot_state::RobotState* state, const robot_model::JointModelGroup* jmg, const double* joint_positions) { + auto is_valid = [sandbox_scene, ignore_collisions, min_solution_distance, + &ik_solutions](robot_state::RobotState* state, const robot_model::JointModelGroup* jmg, + const double* joint_positions) { for (const auto& sol : ik_solutions) { if (jmg->distance(joint_positions, sol.data()) < min_solution_distance) return false; // too close to already found solution diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp index 405adacd1..2e5186080 100644 --- a/core/src/stages/fix_collision_objects.cpp +++ b/core/src/stages/fix_collision_objects.cpp @@ -72,8 +72,8 @@ bool computeCorrection(const std::vector& contacts, Eigen::Vector3d correction.setZero(); for (const cd::Contact& c : contacts) { if ((c.body_type_1 != cd::BodyTypes::WORLD_OBJECT && c.body_type_2 != cd::BodyTypes::WORLD_OBJECT)) { - ROS_WARN_STREAM_NAMED("FixCollisionObjects", "Cannot fix collision between " << c.body_name_1 << " and " - << c.body_name_2); + ROS_WARN_STREAM_NAMED("FixCollisionObjects", + "Cannot fix collision between " << c.body_name_1 << " and " << c.body_name_2); return false; } if (c.body_type_1 == cd::BodyTypes::WORLD_OBJECT) diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 86eeea927..d161b0361 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -125,7 +125,7 @@ bool MoveTo::getJointStateGoal(const boost::any& goal, const moveit::core::Joint bool MoveTo::getPoseGoal(const boost::any& goal, const geometry_msgs::PoseStamped& ik_pose_msg, const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen, - decltype(std::declval().markers()) & markers) { + decltype(std::declval().markers())& markers) { try { const geometry_msgs::PoseStamped& target = boost::any_cast(goal); tf::poseMsgToEigen(target.pose, target_eigen); @@ -147,7 +147,7 @@ bool MoveTo::getPoseGoal(const boost::any& goal, const geometry_msgs::PoseStampe bool MoveTo::getPointGoal(const boost::any& goal, const moveit::core::LinkModel* link, const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen, - decltype(std::declval().markers()) & /*unused*/) { + decltype(std::declval().markers())& /*unused*/) { try { const geometry_msgs::PointStamped& target = boost::any_cast(goal); Eigen::Vector3d target_point; diff --git a/core/src/task.cpp b/core/src/task.cpp index 16d5af052..83f096c4f 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -117,7 +117,7 @@ Task::Task(const std::string& id, bool introspection, ContainerBase::pointer&& c } Task::Task(Task&& other) // NOLINT(performance-noexcept-move-constructor) - : WrapperBase(new TaskPrivate(this, std::string()), std::make_unique()) { + : WrapperBase(new TaskPrivate(this, std::string()), std::make_unique()) { *this = std::move(other); } @@ -223,7 +223,7 @@ void Task::enableIntrospection(bool enable) { [](Stage& stage, int /*depth*/) { stage.pimpl()->setIntrospection(nullptr); return true; - }, + }, 1, UINT_MAX); impl->introspection_.reset(); } @@ -275,7 +275,7 @@ void Task::init() { [impl](Stage& stage, int /*depth*/) { stage.pimpl()->setIntrospection(impl->introspection_.get()); return true; - }, + }, 1, UINT_MAX); // first time publish task diff --git a/core/test/models.h b/core/test/models.h index 8a3d7b208..a33e3377d 100644 --- a/core/test/models.h +++ b/core/test/models.h @@ -6,7 +6,7 @@ namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotModel) } -} +} // namespace moveit // get a hard-coded model moveit::core::RobotModelPtr getModel(); diff --git a/visualization/motion_planning_tasks/properties/property_factory.h b/visualization/motion_planning_tasks/properties/property_factory.h index d78cd8fde..84c7c9167 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.h +++ b/visualization/motion_planning_tasks/properties/property_factory.h @@ -48,7 +48,7 @@ namespace rviz { class Property; class PropertyTreeModel; class DisplayContext; -} +} // namespace rviz namespace planning_scene { class PlanningScene; } @@ -56,7 +56,7 @@ namespace moveit { namespace task_constructor { class Stage; } -} +} // namespace moveit namespace moveit_rviz_plugin { diff --git a/visualization/motion_planning_tasks/src/icons.cpp b/visualization/motion_planning_tasks/src/icons.cpp index 190279411..1b5038c1d 100644 --- a/visualization/motion_planning_tasks/src/icons.cpp +++ b/visualization/motion_planning_tasks/src/icons.cpp @@ -11,5 +11,5 @@ const Icon FORWARD({ { QLatin1String(":icons/downarrow.png"), QColor::fromRgba(0 const Icon BACKWARD({ { QLatin1String(":icons/uparrow.png"), QColor::fromRgba(0xff000000) } }, Icon::TINT); const Icon BOTHWAY({ { QLatin1String(":icons/updownarrow.png"), QColor::fromRgba(0xff000000) } }, Icon::TINT); const Icon GENERATE({ { QLatin1String(":icons/generatearrow.png"), QColor::fromRgba(0xff000000) } }, Icon::TINT); -} -} +} // namespace icons +} // namespace moveit_rviz_plugin diff --git a/visualization/motion_planning_tasks/src/icons.h b/visualization/motion_planning_tasks/src/icons.h index 1204eb3e3..a1ab52cb4 100644 --- a/visualization/motion_planning_tasks/src/icons.h +++ b/visualization/motion_planning_tasks/src/icons.h @@ -10,5 +10,5 @@ extern const moveit_rviz_plugin::utils::Icon FORWARD; extern const moveit_rviz_plugin::utils::Icon BACKWARD; extern const moveit_rviz_plugin::utils::Icon BOTHWAY; extern const moveit_rviz_plugin::utils::Icon GENERATE; -} -} +} // namespace icons +} // namespace moveit_rviz_plugin diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index 0fb533b70..565037c84 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -596,7 +596,7 @@ void RemoteSolutionModel::sortInternal() { if (comp == 0) // if still undecided, id decides comp = (left->id < right->id ? -1 : 1); return (sort_order_ == Qt::AscendingOrder) ? (comp < 0) : (comp >= 0); - }); + }); } // map old indexes to new ones diff --git a/visualization/motion_planning_tasks/src/task_display.h b/visualization/motion_planning_tasks/src/task_display.h index 88c5a1aca..a220079ad 100644 --- a/visualization/motion_planning_tasks/src/task_display.h +++ b/visualization/motion_planning_tasks/src/task_display.h @@ -54,13 +54,13 @@ namespace rviz { class StringProperty; class RosTopicProperty; -} +} // namespace rviz namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotModel) } -} +} // namespace moveit namespace rdf_loader { MOVEIT_CLASS_FORWARD(RDFLoader) } diff --git a/visualization/motion_planning_tasks/src/task_list_model.h b/visualization/motion_planning_tasks/src/task_list_model.h index 4fba79e1f..f18c38171 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.h +++ b/visualization/motion_planning_tasks/src/task_list_model.h @@ -58,7 +58,7 @@ class ServiceClient; namespace rviz { class PropertyTreeModel; class DisplayContext; -} +} // namespace rviz namespace moveit_rviz_plugin { diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index 9084c03e7..08cf33bd9 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -226,7 +226,7 @@ TaskViewPrivate::TaskViewPrivate(TaskView* q_ptr) : q_ptr(q_ptr), exec_action_cl } tasks_view->setExpanded(parent, true); // expand parent group item } - }); + }); tasks_view->setSelectionMode(QAbstractItemView::ExtendedSelection); tasks_view->setAcceptDrops(true); diff --git a/visualization/motion_planning_tasks/src/task_panel.h b/visualization/motion_planning_tasks/src/task_panel.h index 20df43e95..6cf9a05a6 100644 --- a/visualization/motion_planning_tasks/src/task_panel.h +++ b/visualization/motion_planning_tasks/src/task_panel.h @@ -49,7 +49,7 @@ class WindowManagerInterface; class Property; class BoolProperty; class EnumProperty; -} +} // namespace rviz namespace moveit_rviz_plugin { @@ -108,7 +108,7 @@ class MetaTaskListModel; * Subscribing to task_monitoring and task_solution topics, it collects information * about running tasks and their solutions and allows to inspect both, * successful solutions and failed solution attempts. -*/ + */ class TaskViewPrivate; class TaskView : public SubPanel { diff --git a/visualization/motion_planning_tasks/test/test_merge_models.cpp b/visualization/motion_planning_tasks/test/test_merge_models.cpp index db2e415e7..e31a0641d 100644 --- a/visualization/motion_planning_tasks/test/test_merge_models.cpp +++ b/visualization/motion_planning_tasks/test/test_merge_models.cpp @@ -61,17 +61,18 @@ void modifySourceModel(QAbstractItemModel* src, T* proxy, const QModelIndex& src const char* new_value = "foo"; #if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0) - auto con = proxy->connect(proxy, &T::dataChanged, [&called, &src_index, new_value](const QModelIndex& topLeft, - const QModelIndex& bottomRight) { - EXPECT_EQ(topLeft.row(), src_index.row()); - EXPECT_EQ(topLeft.column(), src_index.column()); - - EXPECT_EQ(bottomRight.row(), src_index.row()); - EXPECT_EQ(bottomRight.column(), src_index.column()); - - EXPECT_STREQ(topLeft.data().toString().toLatin1().data(), new_value); - called = true; - }); + auto con = + proxy->connect(proxy, &T::dataChanged, + [&called, &src_index, new_value](const QModelIndex& topLeft, const QModelIndex& bottomRight) { + EXPECT_EQ(topLeft.row(), src_index.row()); + EXPECT_EQ(topLeft.column(), src_index.column()); + + EXPECT_EQ(bottomRight.row(), src_index.row()); + EXPECT_EQ(bottomRight.column(), src_index.column()); + + EXPECT_STREQ(topLeft.data().toString().toLatin1().data(), new_value); + called = true; + }); #endif EXPECT_TRUE(src->setData(src_index, new_value)); diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h index 17a1c104d..20882bf11 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h @@ -43,7 +43,7 @@ namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotState) } -} +} // namespace moveit namespace planning_scene { MOVEIT_CLASS_FORWARD(PlanningScene) } diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h index da74584fd..09a7cdc48 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h @@ -14,7 +14,7 @@ class SceneNode; namespace rviz { class DisplayContext; class MarkerBase; -} +} // namespace rviz namespace planning_scene { MOVEIT_CLASS_FORWARD(PlanningScene) @@ -23,7 +23,7 @@ namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotState) } -} +} // namespace moveit namespace moveit_rviz_plugin { diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h index 93bd88f64..ee53493ac 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h @@ -67,7 +67,7 @@ namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotModel) } -} +} // namespace moveit namespace planning_scene { MOVEIT_CLASS_FORWARD(PlanningScene) } diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index 317af1608..813c5eab4 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -72,10 +72,10 @@ namespace moveit_rviz_plugin { TaskSolutionVisualization::TaskSolutionVisualization(rviz::Property* parent, rviz::Display* display) : display_(display) { // trajectory properties - interrupt_display_property_ = - new rviz::BoolProperty("Interrupt Display", false, "Immediately show newly planned trajectory, " - "interrupting the currently displayed one.", - parent); + interrupt_display_property_ = new rviz::BoolProperty("Interrupt Display", false, + "Immediately show newly planned trajectory, " + "interrupting the currently displayed one.", + parent); loop_display_property_ = new rviz::BoolProperty( "Loop Animation", false, "Indicates whether the last received path is to be animated in a loop", parent, @@ -84,10 +84,11 @@ TaskSolutionVisualization::TaskSolutionVisualization(rviz::Property* parent, rvi trail_display_property_ = new rviz::BoolProperty("Show Trail", false, "Show a path trail", parent, SLOT(changedTrail()), this); - state_display_time_property_ = new rviz::EditableEnumProperty( - "State Display Time", "0.05 s", "The amount of wall-time to wait in between displaying " - "states along a received trajectory path", - parent); + state_display_time_property_ = + new rviz::EditableEnumProperty("State Display Time", "0.05 s", + "The amount of wall-time to wait in between displaying " + "states along a received trajectory path", + parent); state_display_time_property_->addOptionStd("REALTIME"); state_display_time_property_->addOptionStd("0.05 s"); state_display_time_property_->addOptionStd("0.1 s"); @@ -100,14 +101,15 @@ TaskSolutionVisualization::TaskSolutionVisualization(rviz::Property* parent, rvi // robot properties robot_property_ = new rviz::Property("Robot", QString(), QString(), parent); - robot_visual_enabled_property_ = - new rviz::BoolProperty("Show Robot Visual", true, "Indicates whether the geometry of the robot as defined for " - "visualisation purposes should be displayed", - robot_property_, SLOT(changedRobotVisualEnabled()), this); + robot_visual_enabled_property_ = new rviz::BoolProperty("Show Robot Visual", true, + "Indicates whether the geometry of the robot as defined for " + "visualisation purposes should be displayed", + robot_property_, SLOT(changedRobotVisualEnabled()), this); robot_collision_enabled_property_ = - new rviz::BoolProperty("Show Robot Collision", false, "Indicates whether the geometry of the robot as defined " - "for collision detection purposes should be displayed", + new rviz::BoolProperty("Show Robot Collision", false, + "Indicates whether the geometry of the robot as defined " + "for collision detection purposes should be displayed", robot_property_, SLOT(changedRobotCollisionEnabled()), this); robot_alpha_property_ = new rviz::FloatProperty("Robot Alpha", 0.5f, "Specifies the alpha for the robot links", @@ -119,10 +121,10 @@ TaskSolutionVisualization::TaskSolutionVisualization(rviz::Property* parent, rvi new rviz::ColorProperty("Fixed Robot Color", QColor(150, 50, 150), "The color of the animated robot", robot_property_, SLOT(changedRobotColor()), this); - enable_robot_color_property_ = - new rviz::BoolProperty("Use Fixed Robot Color", false, "Specifies whether the fixed robot color should be used." - " If not, the original color is used.", - robot_property_, SLOT(enabledRobotColor()), this); + enable_robot_color_property_ = new rviz::BoolProperty("Use Fixed Robot Color", false, + "Specifies whether the fixed robot color should be used." + " If not, the original color is used.", + robot_property_, SLOT(enabledRobotColor()), this); // planning scene properties scene_enabled_property_ = @@ -404,9 +406,8 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt) { current_state_time_ = 0.0; trail_scene_node_->setVisible(false); } else if (tm < 0.0) { // using realtime: skip to next waypoint based on elapsed display time - while (current_state_ < max_state_index && - (tm = displaying_solution_->getWayPointDurationFromPrevious(current_state_ + 1)) < - current_state_time_) { + while (current_state_ < max_state_index && (tm = displaying_solution_->getWayPointDurationFromPrevious( + current_state_ + 1)) < current_state_time_) { ++current_state_; current_state_time_ -= tm; } @@ -618,9 +619,10 @@ void TaskSolutionVisualization::setVisibility(Ogre::SceneNode* node, Ogre::Scene void TaskSolutionVisualization::setVisibility() { // main scene node is visible if animation, trail, or panel is shown, or if display is locked - setVisibility(main_scene_node_, parent_scene_node_, display_->isEnabled() && displaying_solution_ && - (animating_ || locked_ || trail_scene_node_->getParent() || - (slider_panel_ && slider_panel_->isVisible()))); + setVisibility( + main_scene_node_, parent_scene_node_, + display_->isEnabled() && displaying_solution_ && + (animating_ || locked_ || trail_scene_node_->getParent() || (slider_panel_ && slider_panel_->isVisible()))); } } // namespace moveit_rviz_plugin