From 97648c6ce7c7c319a448a308e6dad815cea173e0 Mon Sep 17 00:00:00 2001 From: Mark Moll Date: Tue, 1 Sep 2020 10:52:37 -0700 Subject: [PATCH 1/4] small compilation fixes for macOS --- .../collision_detection_fcl/CMakeLists.txt | 1 + moveit_core/utils/CMakeLists.txt | 1 + .../kdl_kinematics_plugin/CMakeLists.txt | 1 + .../lma_kinematics_plugin/CMakeLists.txt | 1 + moveit_ros/move_group/CMakeLists.txt | 1 + .../cartesian_path_service_capability.cpp | 2 +- .../kinematics_service_capability.cpp | 2 +- .../move_action_capability.cpp | 4 ++-- .../src/kinematics_plugin_loader.cpp | 4 ++-- .../plan_execution/src/plan_execution.cpp | 6 +++--- .../planning/planning_pipeline/CMakeLists.txt | 1 + .../src/planning_scene_monitor.cpp | 20 +++++++++---------- .../src/interaction_handler.cpp | 13 ++++++------ .../src/robot_interaction.cpp | 2 +- .../test/locked_robot_state_test.cpp | 2 +- moveit_ros/visualization/CMakeLists.txt | 2 ++ .../planning_scene_rviz_plugin/CMakeLists.txt | 4 ++++ .../src/planning_scene_display.cpp | 2 +- .../robot_state_rviz_plugin/CMakeLists.txt | 4 ++++ .../rviz_plugin_render_tools/CMakeLists.txt | 2 ++ .../trajectory_rviz_plugin/CMakeLists.txt | 4 ++++ 21 files changed, 51 insertions(+), 28 deletions(-) diff --git a/moveit_core/collision_detection_fcl/CMakeLists.txt b/moveit_core/collision_detection_fcl/CMakeLists.txt index 415917cd07..04da3f3eca 100644 --- a/moveit_core/collision_detection_fcl/CMakeLists.txt +++ b/moveit_core/collision_detection_fcl/CMakeLists.txt @@ -30,6 +30,7 @@ ament_target_dependencies(collision_detector_fcl_plugin ) target_link_libraries(collision_detector_fcl_plugin ${MOVEIT_LIB_NAME} + moveit_planning_scene ) install(TARGETS ${MOVEIT_LIB_NAME} collision_detector_fcl_plugin diff --git a/moveit_core/utils/CMakeLists.txt b/moveit_core/utils/CMakeLists.txt index 525be9a05c..26ae41cff0 100644 --- a/moveit_core/utils/CMakeLists.txt +++ b/moveit_core/utils/CMakeLists.txt @@ -23,6 +23,7 @@ if(BUILD_TESTING) add_library(${MOVEIT_TEST_LIB_NAME} SHARED src/robot_model_test_utils.cpp) target_link_libraries(${MOVEIT_TEST_LIB_NAME} moveit_robot_model) ament_target_dependencies(${MOVEIT_TEST_LIB_NAME} + ament_index_cpp Boost geometry_msgs urdf diff --git a/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt index 0762e5d640..46e4b667d5 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt +++ b/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt @@ -12,6 +12,7 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} moveit_msgs orocos_kdl kdl_parser + tf2_kdl EIGEN3 ) diff --git a/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt index 3a5ad9cd72..813abbf69a 100644 --- a/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt +++ b/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt @@ -7,6 +7,7 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp moveit_core moveit_msgs + tf2_kdl ) install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION lib) diff --git a/moveit_ros/move_group/CMakeLists.txt b/moveit_ros/move_group/CMakeLists.txt index 38748abbb2..0f44e63be2 100644 --- a/moveit_ros/move_group/CMakeLists.txt +++ b/moveit_ros/move_group/CMakeLists.txt @@ -59,6 +59,7 @@ set_target_properties(moveit_move_group_default_capabilities target_include_directories(moveit_move_group_default_capabilities PUBLIC include) ament_target_dependencies(moveit_move_group_default_capabilities rclcpp rclcpp_action moveit_core moveit_ros_planning std_srvs) +target_link_libraries(moveit_move_group_default_capabilities moveit_move_group_capabilities_base) install(TARGETS move_group list_move_group_capabilities RUNTIME diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp index c6f9e2497c..8a502a3750 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp @@ -149,7 +149,7 @@ bool MoveGroupCartesianPathService::computeService( constraint_fn = boost::bind( &isStateValid, req->avoid_collisions ? static_cast(*ls).get() : nullptr, - kset->empty() ? nullptr : kset.get(), _1, _2, _3); + kset->empty() ? nullptr : kset.get(), boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3); } bool global_frame = !moveit::core::Transforms::sameFrame(link_name, req->header.frame_id); RCLCPP_INFO(LOGGER, "Attempting to follow %u waypoints for link '%s' using a step of %lf m " diff --git a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp index ba853dd6ef..dc46a35120 100644 --- a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp @@ -167,7 +167,7 @@ bool MoveGroupKinematicsService::computeIKService(const std::shared_ptrik_request.avoid_collisions ? static_cast(ls).get() : nullptr, - kset.empty() ? nullptr : &kset, _1, _2, _3)); + kset.empty() ? nullptr : &kset, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } else { diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp index 6fb5ab5fcc..accf38ebbb 100644 --- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp @@ -162,11 +162,11 @@ void MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const std::shared_pt opt.before_execution_callback_ = boost::bind(&MoveGroupMoveAction::startMoveExecutionCallback, this); opt.plan_callback_ = - boost::bind(&MoveGroupMoveAction::planUsingPlanningPipeline, this, boost::cref(motion_plan_request), _1); + boost::bind(&MoveGroupMoveAction::planUsingPlanningPipeline, this, boost::cref(motion_plan_request), boost::placeholders::_1); if (goal->get_goal()->planning_options.look_around && context_->plan_with_sensing_) { opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(), - _1, opt.plan_callback_, goal->get_goal()->planning_options.look_around_attempts, + boost::placeholders::_1, opt.plan_callback_, goal->get_goal()->planning_options.look_around_attempts, goal->get_goal()->planning_options.max_safe_execution_cost); context_->plan_with_sensing_->setBeforeLookCallback(boost::bind(&MoveGroupMoveAction::startMoveLookCallback, this)); } diff --git a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp index c9c444457f..89a37df918 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp +++ b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp @@ -260,7 +260,7 @@ moveit::core::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction() moveit::tools::Profiler::ScopedBlock prof_block("KinematicsPluginLoader::getLoaderFunction"); if (loader_) - return boost::bind(&KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(), _1); + return boost::bind(&KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(), boost::placeholders::_1); rdf_loader::RDFLoader rml(node_, robot_description_); robot_description_ = rml.getRobotDescription(); @@ -417,6 +417,6 @@ moveit::core::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction(const iksolver_to_tip_links)); } - return boost::bind(&KinematicsPluginLoader::KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(), _1); + return boost::bind(&KinematicsPluginLoader::KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(), boost::placeholders::_1); } } // namespace kinematics_plugin_loader diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index e907ee703b..e693a88219 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -86,7 +86,7 @@ plan_execution::PlanExecution::PlanExecution( new_scene_update_ = false; // we want to be notified when new information is available - planning_scene_monitor_->addUpdateCallback(boost::bind(&PlanExecution::planningSceneUpdatedCallback, this, _1)); + planning_scene_monitor_->addUpdateCallback(boost::bind(&PlanExecution::planningSceneUpdatedCallback, this, boost::placeholders::_1)); // start the dynamic-reconfigure server // reconfigure_impl_ = new DynamicReconfigureImpl(this); @@ -402,8 +402,8 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni // start a trajectory execution thread trajectory_execution_manager_->execute( - boost::bind(&PlanExecution::doneWithTrajectoryExecution, this, _1), - boost::bind(&PlanExecution::successfulTrajectorySegmentExecution, this, &plan, _1)); + boost::bind(&PlanExecution::doneWithTrajectoryExecution, this, boost::placeholders::_1), + boost::bind(&PlanExecution::successfulTrajectorySegmentExecution, this, &plan, boost::placeholders::_1)); // wait for path to be done, while checking that the path does not become invalid rclcpp::WallRate r(100); path_became_invalid_ = false; diff --git a/moveit_ros/planning/planning_pipeline/CMakeLists.txt b/moveit_ros/planning/planning_pipeline/CMakeLists.txt index 406f0bf6bb..3ef0122175 100644 --- a/moveit_ros/planning/planning_pipeline/CMakeLists.txt +++ b/moveit_ros/planning/planning_pipeline/CMakeLists.txt @@ -8,6 +8,7 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} moveit_msgs rclcpp Boost + pluginlib ) install(TARGETS ${MOVEIT_LIB_NAME} diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index eceec4b1e9..55fc3c76f5 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -184,9 +184,9 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc if (scene_) { scene_->setAttachedBodyUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2)); + boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, boost::placeholders::_1, boost::placeholders::_2)); scene_->setCollisionObjectUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2)); + boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, boost::placeholders::_1, boost::placeholders::_2)); } } else @@ -322,9 +322,9 @@ void PlanningSceneMonitor::monitorDiffs(bool flag) scene_ = parent_scene_->diff(); scene_const_ = scene_; scene_->setAttachedBodyUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2)); + boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, boost::placeholders::_1, boost::placeholders::_2)); scene_->setCollisionObjectUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2)); + boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, boost::placeholders::_1, boost::placeholders::_2)); } } else @@ -435,9 +435,9 @@ void PlanningSceneMonitor::scenePublishingThread() scene_->pushDiffs(parent_scene_); scene_->clearDiffs(); scene_->setAttachedBodyUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2)); + boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, boost::placeholders::_1, boost::placeholders::_2)); scene_->setCollisionObjectUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2)); + boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, boost::placeholders::_1, boost::placeholders::_2)); if (octomap_monitor_) { excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in @@ -667,9 +667,9 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann scene_ = parent_scene_->diff(); scene_const_ = scene_; scene_->setAttachedBodyUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2)); + boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, boost::placeholders::_1, boost::placeholders::_2)); scene_->setCollisionObjectUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2)); + boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, boost::placeholders::_1, boost::placeholders::_2)); } if (octomap_monitor_) { @@ -1174,7 +1174,7 @@ void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collisio excludeWorldObjectsFromOctree(); octomap_monitor_->setTransformCacheCallback( - boost::bind(&PlanningSceneMonitor::getShapeTransformCache, this, _1, _2, _3)); + boost::bind(&PlanningSceneMonitor::getShapeTransformCache, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); octomap_monitor_->setUpdateCallback(boost::bind(&PlanningSceneMonitor::octomapUpdateCallback, this)); } octomap_monitor_->startMonitor(); @@ -1207,7 +1207,7 @@ void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_top { current_state_monitor_.reset(new CurrentStateMonitor(pnode_, getRobotModel(), tf_buffer_)); } - current_state_monitor_->addUpdateCallback(boost::bind(&PlanningSceneMonitor::onStateUpdate, this, _1)); + current_state_monitor_->addUpdateCallback(boost::bind(&PlanningSceneMonitor::onStateUpdate, this, boost::placeholders::_1)); current_state_monitor_->startStateMonitor(joint_states_topic); { diff --git a/moveit_ros/robot_interaction/src/interaction_handler.cpp b/moveit_ros/robot_interaction/src/interaction_handler.cpp index c59bb3ae6d..f601782e2b 100644 --- a/moveit_ros/robot_interaction/src/interaction_handler.cpp +++ b/moveit_ros/robot_interaction/src/interaction_handler.cpp @@ -1,3 +1,4 @@ + /********************************************************************* * Software License Agreement (BSD License) * @@ -210,7 +211,7 @@ void InteractionHandler::handleGeneric( StateChangeCallbackFn callback; // modify the RobotState in-place with the state_lock_ held. LockedRobotState::modifyState( - boost::bind(&InteractionHandler::updateStateGeneric, this, _1, &g, &feedback, &callback)); + boost::bind(&InteractionHandler::updateStateGeneric, this, boost::placeholders::_1, &g, &feedback, &callback)); // This calls update_callback_ to notify client that state changed. if (callback) @@ -243,7 +244,7 @@ void InteractionHandler::handleEndEffector( // modify the RobotState in-place with state_lock_ held. // This locks state_lock_ before calling updateState() LockedRobotState::modifyState( - boost::bind(&InteractionHandler::updateStateEndEffector, this, _1, &eef, &tpose.pose, &callback)); + boost::bind(&InteractionHandler::updateStateEndEffector, this, boost::placeholders::_1, &eef, &tpose.pose, &callback)); // This calls update_callback_ to notify client that state changed. if (callback) @@ -274,7 +275,7 @@ void InteractionHandler::handleJoint(const JointInteraction& vj, // modify the RobotState in-place with state_lock_ held. // This locks state_lock_ before calling updateState() LockedRobotState::modifyState( - boost::bind(&InteractionHandler::updateStateJoint, this, _1, &vj, &tpose.pose, &callback)); + boost::bind(&InteractionHandler::updateStateJoint, this, boost::placeholders::_1, &vj, &tpose.pose, &callback)); // This calls update_callback_ to notify client that state changed. if (callback) @@ -289,7 +290,7 @@ void InteractionHandler::updateStateGeneric( bool ok = g->process_feedback(*state, *feedback); bool error_state_changed = setErrorState(g->marker_name_suffix, !ok); if (update_callback_) - *callback = boost::bind(update_callback_, _1, error_state_changed); + *callback = boost::bind(update_callback_, boost::placeholders::_1, error_state_changed); } // MUST hold state_lock_ when calling this! @@ -303,7 +304,7 @@ void InteractionHandler::updateStateEndEffector(moveit::core::RobotState* state, bool ok = kinematic_options.setStateFromIK(*state, eef->parent_group, eef->parent_link, *pose); bool error_state_changed = setErrorState(eef->parent_group, !ok); if (update_callback_) - *callback = boost::bind(update_callback_, _1, error_state_changed); + *callback = boost::bind(update_callback_, boost::placeholders::_1, error_state_changed); } // MUST hold state_lock_ when calling this! @@ -321,7 +322,7 @@ void InteractionHandler::updateStateJoint(moveit::core::RobotState* state, const state->update(); if (update_callback_) - *callback = boost::bind(update_callback_, _1, false); + *callback = boost::bind(update_callback_, boost::placeholders::_1, false); } bool InteractionHandler::inError(const EndEffectorInteraction& eef) const diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp index 2f8621f047..54eb1df0f7 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -543,7 +543,7 @@ void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handle { int_marker_server_->insert(im); int_marker_server_->setCallback(im.name, - boost::bind(&RobotInteraction::processInteractiveMarkerFeedback, this, _1)); + boost::bind(&RobotInteraction::processInteractiveMarkerFeedback, this, boost::placeholders::_1)); // Add menu handler to all markers that this interaction handler creates. if (std::shared_ptr mh = handler->getMenuHandler()) diff --git a/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp b/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp index dc820d047f..a96f29f77c 100644 --- a/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp +++ b/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp @@ -433,7 +433,7 @@ void MyInfo::modifyThreadFunc(robot_interaction::LockedRobotState* locked_state, { val += 0.0001; - locked_state->modifyState(boost::bind(&MyInfo::modifyFunc, this, _1, val)); + locked_state->modifyState(boost::bind(&MyInfo::modifyFunc, this, boost::placeholders::_1, val)); } cnt_lock_.lock(); diff --git a/moveit_ros/visualization/CMakeLists.txt b/moveit_ros/visualization/CMakeLists.txt index 3ffed9b3c1..fcad727d3b 100644 --- a/moveit_ros/visualization/CMakeLists.txt +++ b/moveit_ros/visualization/CMakeLists.txt @@ -101,3 +101,5 @@ ament_export_dependencies(tf2_eigen) ament_export_dependencies(Eigen3) ament_export_dependencies(rviz_ogre_vendor) ament_package() + +include_directories("${OGRE_PREFIX_DIR}/include") diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt index 72a4a50be8..7307aa383c 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt @@ -13,14 +13,18 @@ ament_target_dependencies(${MOVEIT_LIB_NAME}_core moveit_msgs pluginlib Boost + rviz_ogre_vendor ) +target_include_directories(${MOVEIT_LIB_NAME}_core PRIVATE "${OGRE_PREFIX_DIR}/include") add_library(${MOVEIT_LIB_NAME} SHARED src/plugin_init.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core Qt5::Widgets) ament_target_dependencies(${MOVEIT_LIB_NAME} pluginlib + rviz_ogre_vendor ) +target_include_directories(${MOVEIT_LIB_NAME} PRIVATE "${OGRE_PREFIX_DIR}/include") install(DIRECTORY include/ DESTINATION include) diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index deb4c95c19..dec2c552d6 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -542,7 +542,7 @@ void PlanningSceneDisplay::loadRobotModel() { planning_scene_monitor_.swap(psm); planning_scene_monitor_->addUpdateCallback( - boost::bind(&PlanningSceneDisplay::sceneMonitorReceivedUpdate, this, _1)); + boost::bind(&PlanningSceneDisplay::sceneMonitorReceivedUpdate, this, boost::placeholders::_1)); addMainLoopJob(boost::bind(&PlanningSceneDisplay::onRobotModelLoaded, this)); setStatus(rviz_common::properties::StatusProperty::Ok, "PlanningScene", "Planning Scene Loaded Successfully"); waitForAllMainLoopJobs(); diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt index f2a4e6bb56..d7f002d90e 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt @@ -13,7 +13,9 @@ ament_target_dependencies(${MOVEIT_LIB_NAME}_core moveit_msgs pluginlib Boost + rviz_ogre_vendor ) +target_include_directories(${MOVEIT_LIB_NAME}_core PRIVATE "${OGRE_PREFIX_DIR}/include") add_library(${MOVEIT_LIB_NAME} SHARED src/plugin_init.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") @@ -22,7 +24,9 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp pluginlib Boost + rviz_ogre_vendor ) +target_include_directories(${MOVEIT_LIB_NAME} PRIVATE "${OGRE_PREFIX_DIR}/include") install(DIRECTORY include/ DESTINATION include) diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt b/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt index 97e697ac4d..bec1f076c5 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt +++ b/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt @@ -37,7 +37,9 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} moveit_core Boost octomap_msgs + rviz_ogre_vendor ) +target_include_directories(${MOVEIT_LIB_NAME} PRIVATE "${OGRE_PREFIX_DIR}/include") install(DIRECTORY include/ DESTINATION include) diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt index d394a8bc6c..d970da24f6 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt @@ -25,7 +25,9 @@ ament_target_dependencies(${MOVEIT_LIB_NAME}_core moveit_msgs pluginlib Boost + rviz_ogre_vendor ) +target_include_directories(${MOVEIT_LIB_NAME}_core PRIVATE "${OGRE_PREFIX_DIR}/include") # Plugin Initializer add_library(${MOVEIT_LIB_NAME} SHARED src/plugin_init.cpp) @@ -35,7 +37,9 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp pluginlib Boost + rviz_ogre_vendor ) +target_include_directories(${MOVEIT_LIB_NAME} PRIVATE "${OGRE_PREFIX_DIR}/include") install(DIRECTORY include/ DESTINATION include) From 4e3ac467d66f3291fb54fb55e77b4026864222f2 Mon Sep 17 00:00:00 2001 From: Mark Moll Date: Thu, 3 Sep 2020 18:38:05 -0500 Subject: [PATCH 2/4] clang-format --- .../cartesian_path_service_capability.cpp | 3 +- .../kinematics_service_capability.cpp | 3 +- .../move_action_capability.cpp | 7 ++-- .../src/kinematics_plugin_loader.cpp | 3 +- .../plan_execution/src/plan_execution.cpp | 3 +- .../src/planning_scene_monitor.cpp | 39 ++++++++++--------- .../src/interaction_handler.cpp | 4 +- .../src/robot_interaction.cpp | 4 +- 8 files changed, 37 insertions(+), 29 deletions(-) diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp index 8a502a3750..058c646394 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp @@ -149,7 +149,8 @@ bool MoveGroupCartesianPathService::computeService( constraint_fn = boost::bind( &isStateValid, req->avoid_collisions ? static_cast(*ls).get() : nullptr, - kset->empty() ? nullptr : kset.get(), boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3); + kset->empty() ? nullptr : kset.get(), boost::placeholders::_1, boost::placeholders::_2, + boost::placeholders::_3); } bool global_frame = !moveit::core::Transforms::sameFrame(link_name, req->header.frame_id); RCLCPP_INFO(LOGGER, "Attempting to follow %u waypoints for link '%s' using a step of %lf m " diff --git a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp index dc46a35120..780a3ead3a 100644 --- a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp @@ -167,7 +167,8 @@ bool MoveGroupKinematicsService::computeIKService(const std::shared_ptrik_request.avoid_collisions ? static_cast(ls).get() : nullptr, - kset.empty() ? nullptr : &kset, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); + kset.empty() ? nullptr : &kset, boost::placeholders::_1, boost::placeholders::_2, + boost::placeholders::_3)); } else { diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp index accf38ebbb..a2d9ed1721 100644 --- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp @@ -161,12 +161,13 @@ void MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const std::shared_pt opt.replan_delay_ = goal->get_goal()->planning_options.replan_delay; opt.before_execution_callback_ = boost::bind(&MoveGroupMoveAction::startMoveExecutionCallback, this); - opt.plan_callback_ = - boost::bind(&MoveGroupMoveAction::planUsingPlanningPipeline, this, boost::cref(motion_plan_request), boost::placeholders::_1); + opt.plan_callback_ = boost::bind(&MoveGroupMoveAction::planUsingPlanningPipeline, this, + boost::cref(motion_plan_request), boost::placeholders::_1); if (goal->get_goal()->planning_options.look_around && context_->plan_with_sensing_) { opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(), - boost::placeholders::_1, opt.plan_callback_, goal->get_goal()->planning_options.look_around_attempts, + boost::placeholders::_1, opt.plan_callback_, + goal->get_goal()->planning_options.look_around_attempts, goal->get_goal()->planning_options.max_safe_execution_cost); context_->plan_with_sensing_->setBeforeLookCallback(boost::bind(&MoveGroupMoveAction::startMoveLookCallback, this)); } diff --git a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp index 89a37df918..652f402af9 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp +++ b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp @@ -417,6 +417,7 @@ moveit::core::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction(const iksolver_to_tip_links)); } - return boost::bind(&KinematicsPluginLoader::KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(), boost::placeholders::_1); + return boost::bind(&KinematicsPluginLoader::KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(), + boost::placeholders::_1); } } // namespace kinematics_plugin_loader diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index e693a88219..6b2d7b912c 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -86,7 +86,8 @@ plan_execution::PlanExecution::PlanExecution( new_scene_update_ = false; // we want to be notified when new information is available - planning_scene_monitor_->addUpdateCallback(boost::bind(&PlanExecution::planningSceneUpdatedCallback, this, boost::placeholders::_1)); + planning_scene_monitor_->addUpdateCallback( + boost::bind(&PlanExecution::planningSceneUpdatedCallback, this, boost::placeholders::_1)); // start the dynamic-reconfigure server // reconfigure_impl_ = new DynamicReconfigureImpl(this); diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index 55fc3c76f5..839c5fd0f3 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -183,10 +183,10 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc } if (scene_) { - scene_->setAttachedBodyUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, boost::placeholders::_1, boost::placeholders::_2)); - scene_->setCollisionObjectUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, boost::placeholders::_1, boost::placeholders::_2)); + scene_->setAttachedBodyUpdateCallback(boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, + this, boost::placeholders::_1, boost::placeholders::_2)); + scene_->setCollisionObjectUpdateCallback(boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, + this, boost::placeholders::_1, boost::placeholders::_2)); } } else @@ -321,10 +321,10 @@ void PlanningSceneMonitor::monitorDiffs(bool flag) parent_scene_ = scene_; scene_ = parent_scene_->diff(); scene_const_ = scene_; - scene_->setAttachedBodyUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, boost::placeholders::_1, boost::placeholders::_2)); - scene_->setCollisionObjectUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, boost::placeholders::_1, boost::placeholders::_2)); + scene_->setAttachedBodyUpdateCallback(boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, + this, boost::placeholders::_1, boost::placeholders::_2)); + scene_->setCollisionObjectUpdateCallback(boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, + this, boost::placeholders::_1, boost::placeholders::_2)); } } else @@ -435,9 +435,10 @@ void PlanningSceneMonitor::scenePublishingThread() scene_->pushDiffs(parent_scene_); scene_->clearDiffs(); scene_->setAttachedBodyUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, boost::placeholders::_1, boost::placeholders::_2)); - scene_->setCollisionObjectUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, boost::placeholders::_1, boost::placeholders::_2)); + boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, boost::placeholders::_1, + boost::placeholders::_2)); + scene_->setCollisionObjectUpdateCallback(boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, + this, boost::placeholders::_1, boost::placeholders::_2)); if (octomap_monitor_) { excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in @@ -666,10 +667,10 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann parent_scene_ = scene_; scene_ = parent_scene_->diff(); scene_const_ = scene_; - scene_->setAttachedBodyUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, boost::placeholders::_1, boost::placeholders::_2)); - scene_->setCollisionObjectUpdateCallback( - boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, boost::placeholders::_1, boost::placeholders::_2)); + scene_->setAttachedBodyUpdateCallback(boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, + this, boost::placeholders::_1, boost::placeholders::_2)); + scene_->setCollisionObjectUpdateCallback(boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, + this, boost::placeholders::_1, boost::placeholders::_2)); } if (octomap_monitor_) { @@ -1173,8 +1174,9 @@ void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collisio excludeAttachedBodiesFromOctree(); excludeWorldObjectsFromOctree(); - octomap_monitor_->setTransformCacheCallback( - boost::bind(&PlanningSceneMonitor::getShapeTransformCache, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); + octomap_monitor_->setTransformCacheCallback(boost::bind(&PlanningSceneMonitor::getShapeTransformCache, this, + boost::placeholders::_1, boost::placeholders::_2, + boost::placeholders::_3)); octomap_monitor_->setUpdateCallback(boost::bind(&PlanningSceneMonitor::octomapUpdateCallback, this)); } octomap_monitor_->startMonitor(); @@ -1207,7 +1209,8 @@ void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_top { current_state_monitor_.reset(new CurrentStateMonitor(pnode_, getRobotModel(), tf_buffer_)); } - current_state_monitor_->addUpdateCallback(boost::bind(&PlanningSceneMonitor::onStateUpdate, this, boost::placeholders::_1)); + current_state_monitor_->addUpdateCallback( + boost::bind(&PlanningSceneMonitor::onStateUpdate, this, boost::placeholders::_1)); current_state_monitor_->startStateMonitor(joint_states_topic); { diff --git a/moveit_ros/robot_interaction/src/interaction_handler.cpp b/moveit_ros/robot_interaction/src/interaction_handler.cpp index f601782e2b..a524d4c1f1 100644 --- a/moveit_ros/robot_interaction/src/interaction_handler.cpp +++ b/moveit_ros/robot_interaction/src/interaction_handler.cpp @@ -243,8 +243,8 @@ void InteractionHandler::handleEndEffector( // modify the RobotState in-place with state_lock_ held. // This locks state_lock_ before calling updateState() - LockedRobotState::modifyState( - boost::bind(&InteractionHandler::updateStateEndEffector, this, boost::placeholders::_1, &eef, &tpose.pose, &callback)); + LockedRobotState::modifyState(boost::bind(&InteractionHandler::updateStateEndEffector, this, boost::placeholders::_1, + &eef, &tpose.pose, &callback)); // This calls update_callback_ to notify client that state changed. if (callback) diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp index 54eb1df0f7..e5879bff3c 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -542,8 +542,8 @@ void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handle for (const visualization_msgs::msg::InteractiveMarker& im : ims) { int_marker_server_->insert(im); - int_marker_server_->setCallback(im.name, - boost::bind(&RobotInteraction::processInteractiveMarkerFeedback, this, boost::placeholders::_1)); + int_marker_server_->setCallback( + im.name, boost::bind(&RobotInteraction::processInteractiveMarkerFeedback, this, boost::placeholders::_1)); // Add menu handler to all markers that this interaction handler creates. if (std::shared_ptr mh = handler->getMenuHandler()) From 74ce8153c3df17448914b568b25fb2ec5447e56a Mon Sep 17 00:00:00 2001 From: Mark Moll Date: Fri, 4 Sep 2020 16:17:46 -0700 Subject: [PATCH 3/4] more macos fixes --- .../move_group_interface/CMakeLists.txt | 2 +- .../motion_planning_rviz_plugin/CMakeLists.txt | 4 ++++ .../src/motion_planning_display.cpp | 8 ++++---- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt b/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt index 8395bc3fe2..7fef2462e2 100644 --- a/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt @@ -2,7 +2,7 @@ set(MOVEIT_LIB_NAME moveit_move_group_interface) add_library(${MOVEIT_LIB_NAME} SHARED src/move_group_interface.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(${MOVEIT_LIB_NAME} moveit_common_planning_interface_objects moveit_planning_scene_interface) +target_link_libraries(${MOVEIT_LIB_NAME} moveit_common_planning_interface_objects moveit_planning_scene_interface ${Boost_THREAD_LIBRARY}) ament_target_dependencies(${MOVEIT_LIB_NAME} moveit_ros_occupancy_map_monitor moveit_ros_move_group diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt index 46f277c21b..1cafc1fe3f 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt @@ -38,7 +38,9 @@ ament_target_dependencies(${MOVEIT_LIB_NAME}_core rviz_ogre_vendor Qt5 pluginlib + rviz_ogre_vendor ) +target_include_directories(${MOVEIT_LIB_NAME}_core PRIVATE "${OGRE_PREFIX_DIR}/include") add_library(${MOVEIT_LIB_NAME} SHARED src/plugin_init.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") @@ -47,7 +49,9 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} Boost moveit_ros_robot_interaction pluginlib + rviz_ogre_vendor ) +target_include_directories(${MOVEIT_LIB_NAME} PRIVATE "${OGRE_PREFIX_DIR}/include") install(DIRECTORY include/ DESTINATION include) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index 58b306c93d..e79355a5b0 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -177,7 +177,7 @@ MotionPlanningDisplay::MotionPlanningDisplay() trajectory_visual_.reset(new TrajectoryVisualization(path_category_, this)); // Start background jobs - background_process_.setJobUpdateEvent(boost::bind(&MotionPlanningDisplay::backgroundJobUpdate, this, _1, _2)); + background_process_.setJobUpdateEvent(boost::bind(&MotionPlanningDisplay::backgroundJobUpdate, this, boost::placeholders::_1, boost::placeholders::_2)); } // ****************************************************************************************** @@ -1128,7 +1128,7 @@ void MotionPlanningDisplay::onRobotModelLoaded() robot_interaction_.reset( new robot_interaction::RobotInteraction(getRobotModel(), node_, "rviz_moveit_motion_planning_display")); robot_interaction::KinematicOptions o; - o.state_validity_callback_ = boost::bind(&MotionPlanningDisplay::isIKSolutionCollisionFree, this, _1, _2, _3); + o.state_validity_callback_ = boost::bind(&MotionPlanningDisplay::isIKSolutionCollisionFree, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3); robot_interaction_->getKinematicOptionsMap()->setOptions( robot_interaction::KinematicOptionsMap::ALL, o, robot_interaction::KinematicOptions::STATE_VALIDITY_CALLBACK); @@ -1143,8 +1143,8 @@ void MotionPlanningDisplay::onRobotModelLoaded() planning_scene_monitor_->getTFClient())); query_goal_state_.reset(new robot_interaction::InteractionHandler(robot_interaction_, "goal", *previous_state_, planning_scene_monitor_->getTFClient())); - query_start_state_->setUpdateCallback(boost::bind(&MotionPlanningDisplay::scheduleDrawQueryStartState, this, _1, _2)); - query_goal_state_->setUpdateCallback(boost::bind(&MotionPlanningDisplay::scheduleDrawQueryGoalState, this, _1, _2)); + query_start_state_->setUpdateCallback(boost::bind(&MotionPlanningDisplay::scheduleDrawQueryStartState, this, boost::placeholders::_1, boost::placeholders::_2)); + query_goal_state_->setUpdateCallback(boost::bind(&MotionPlanningDisplay::scheduleDrawQueryGoalState, this, boost::placeholders::_1, boost::placeholders::_2)); // Interactive marker menus populateMenuHandler(menu_handler_start_); From 2d689bace118d5dbdd5a06bc5dfe9f190f5305da Mon Sep 17 00:00:00 2001 From: Mark Moll Date: Fri, 4 Sep 2020 18:19:14 -0500 Subject: [PATCH 4/4] clang-format --- .../src/motion_planning_display.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index e79355a5b0..a6dcd49257 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -177,7 +177,8 @@ MotionPlanningDisplay::MotionPlanningDisplay() trajectory_visual_.reset(new TrajectoryVisualization(path_category_, this)); // Start background jobs - background_process_.setJobUpdateEvent(boost::bind(&MotionPlanningDisplay::backgroundJobUpdate, this, boost::placeholders::_1, boost::placeholders::_2)); + background_process_.setJobUpdateEvent( + boost::bind(&MotionPlanningDisplay::backgroundJobUpdate, this, boost::placeholders::_1, boost::placeholders::_2)); } // ****************************************************************************************** @@ -1128,7 +1129,8 @@ void MotionPlanningDisplay::onRobotModelLoaded() robot_interaction_.reset( new robot_interaction::RobotInteraction(getRobotModel(), node_, "rviz_moveit_motion_planning_display")); robot_interaction::KinematicOptions o; - o.state_validity_callback_ = boost::bind(&MotionPlanningDisplay::isIKSolutionCollisionFree, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3); + o.state_validity_callback_ = boost::bind(&MotionPlanningDisplay::isIKSolutionCollisionFree, this, + boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3); robot_interaction_->getKinematicOptionsMap()->setOptions( robot_interaction::KinematicOptionsMap::ALL, o, robot_interaction::KinematicOptions::STATE_VALIDITY_CALLBACK); @@ -1143,8 +1145,10 @@ void MotionPlanningDisplay::onRobotModelLoaded() planning_scene_monitor_->getTFClient())); query_goal_state_.reset(new robot_interaction::InteractionHandler(robot_interaction_, "goal", *previous_state_, planning_scene_monitor_->getTFClient())); - query_start_state_->setUpdateCallback(boost::bind(&MotionPlanningDisplay::scheduleDrawQueryStartState, this, boost::placeholders::_1, boost::placeholders::_2)); - query_goal_state_->setUpdateCallback(boost::bind(&MotionPlanningDisplay::scheduleDrawQueryGoalState, this, boost::placeholders::_1, boost::placeholders::_2)); + query_start_state_->setUpdateCallback(boost::bind(&MotionPlanningDisplay::scheduleDrawQueryStartState, this, + boost::placeholders::_1, boost::placeholders::_2)); + query_goal_state_->setUpdateCallback(boost::bind(&MotionPlanningDisplay::scheduleDrawQueryGoalState, this, + boost::placeholders::_1, boost::placeholders::_2)); // Interactive marker menus populateMenuHandler(menu_handler_start_);