diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index e6207dfe2..984829da7 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -130,9 +130,9 @@ void ExecuteTaskSolutionCapability::preemptCallback() { bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution, plan_execution::ExecutableMotionPlan& plan) { - robot_model::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel(); + moveit::core::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel(); - robot_state::RobotState state(model); + moveit::core::RobotState state(model); { planning_scene_monitor::LockedPlanningSceneRO scene(context_->planning_scene_monitor_); state = scene->getCurrentState(); diff --git a/core/src/merge.cpp b/core/src/merge.cpp index 3bae7a327..e02acb56f 100644 --- a/core/src/merge.cpp +++ b/core/src/merge.cpp @@ -105,7 +105,7 @@ moveit::core::JointModelGroup* merge(const std::vector& sub_trajectories, - const robot_state::RobotState& base_state, moveit::core::JointModelGroup*& merged_group, + const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group, const trajectory_processing::TimeParameterization& time_parameterization) { if (sub_trajectories.size() <= 1) throw std::runtime_error("Expected multiple sub solutions"); @@ -141,7 +141,7 @@ merge(const std::vector& sub_trajecto std::vector values; values.reserve(max_num_vars); - auto merged_state = std::make_shared(base_state); + auto merged_state = std::make_shared(base_state); while (true) { bool finished = true; size_t index = merged_traj->getWayPointCount(); @@ -151,7 +151,7 @@ merge(const std::vector& sub_trajecto continue; // no more waypoints in this sub solution finished = false; // there was a waypoint, continue while loop - const robot_state::RobotState& sub_state = sub->getWayPoint(index); + const moveit::core::RobotState& sub_state = sub->getWayPoint(index); sub_state.copyJointGroupPositions(sub->getGroup(), values); merged_state->setJointGroupPositions(sub->getGroup(), values); } @@ -162,7 +162,7 @@ merge(const std::vector& sub_trajecto // add waypoint without timing merged_traj->addSuffixWayPoint(merged_state, 0.0); // create new RobotState for next waypoint - merged_state = std::make_shared(*merged_state); + merged_state = std::make_shared(*merged_state); } // add timing diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index 4e9df5dc3..36c8bd891 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -89,7 +89,7 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons const double* joint_positions) { state->setJointGroupPositions(jmg, joint_positions); state->update(); - return !sandbox_scene->isStateColliding(const_cast(*state), jmg->getName()) && + return !sandbox_scene->isStateColliding(const_cast(*state), jmg->getName()) && kcs.decide(*state).satisfied; }; diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 333759084..57fd495da 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -52,10 +52,10 @@ struct PlannerCache { using PlannerID = std::tuple; using PlannerMap = std::map >; - using ModelList = std::list, PlannerMap> >; + using ModelList = std::list, PlannerMap> >; ModelList cache_; - PlannerMap::mapped_type& retrieve(const robot_model::RobotModelConstPtr& model, const PlannerID& id) { + PlannerMap::mapped_type& retrieve(const moveit::core::RobotModelConstPtr& model, const PlannerID& id) { // find model in cache_ and remove expired entries while doing so ModelList::iterator model_it = cache_.begin(); while (model_it != cache_.end()) { diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 86307d0c0..738a66178 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -98,11 +98,11 @@ namespace { // ??? TODO: provide callback methods in PlanningScene class / probably not very useful here though... // TODO: move into MoveIt core, lift active_components_only_ from fcl to common interface bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& scene, - robot_state::RobotState& robot_state, Eigen::Isometry3d pose, - const robot_model::LinkModel* link, const robot_model::JointModelGroup* jmg = nullptr, + moveit::core::RobotState& robot_state, Eigen::Isometry3d pose, + const moveit::core::LinkModel* link, const moveit::core::JointModelGroup* jmg = nullptr, collision_detection::CollisionResult* collision_result = nullptr) { // consider all rigidly connected parent links as well - const robot_model::LinkModel* parent = robot_model::RobotModel::getRigidlyConnectedParentLinkModel(link); + const moveit::core::LinkModel* parent = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link); if (parent != link) // transform pose into pose suitable to place parent pose = pose * robot_state.getGlobalLinkTransform(link).inverse() * robot_state.getGlobalLinkTransform(parent); @@ -116,10 +116,10 @@ bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& sce while (parent) { pending_links.push_back(&parent->getName()); link = parent; - const robot_model::JointModel* joint = link->getParentJointModel(); + const moveit::core::JointModel* joint = link->getParentJointModel(); parent = joint->getParentLinkModel(); - if (joint->getType() != robot_model::JointModel::FIXED) { + if (joint->getType() != moveit::core::JointModel::FIXED) { for (const std::string* name : pending_links) acm.setDefaultEntry(*name, true); pending_links.clear(); @@ -280,7 +280,7 @@ void ComputeIK::compute() { } // determine IK link from ik_frame - const robot_model::LinkModel* link = nullptr; + const moveit::core::LinkModel* link = nullptr; geometry_msgs::PoseStamped ik_pose_msg; const boost::any& value = props.get("ik_frame"); if (value.empty()) { // property undefined @@ -311,7 +311,7 @@ void ComputeIK::compute() { // validate placed link for collisions collision_detection::CollisionResult collisions; - robot_state::RobotState sandbox_state{ scene->getCurrentState() }; + moveit::core::RobotState sandbox_state{ scene->getCurrentState() }; bool colliding = !ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, jmg, &collisions); @@ -349,7 +349,7 @@ void ComputeIK::compute() { std::vector compare_pose; const std::string& compare_pose_name = props.get("default_pose"); if (!compare_pose_name.empty()) { - robot_state::RobotState compare_state(robot_model); + moveit::core::RobotState compare_state(robot_model); compare_state.setToDefaultValues(jmg, compare_pose_name); compare_state.copyJointGroupPositions(jmg, compare_pose); } else @@ -359,7 +359,7 @@ void ComputeIK::compute() { IKSolutions ik_solutions; auto is_valid = [scene, ignore_collisions, min_solution_distance, - &ik_solutions](robot_state::RobotState* state, const robot_model::JointModelGroup* jmg, + &ik_solutions](moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg, const double* joint_positions) { for (const auto& sol : ik_solutions) { if (jmg->distance(joint_positions, sol.joint_positions.data()) < min_solution_distance) @@ -419,7 +419,7 @@ void ComputeIK::compute() { solution.markAsFailure(ss.str()); } // set scene's robot state - robot_state::RobotState& solution_state = solution_scene->getCurrentStateNonConst(); + moveit::core::RobotState& solution_state = solution_scene->getCurrentStateNonConst(); solution_state.setJointGroupPositions(jmg, ik_solutions[i].joint_positions.data()); solution_state.update(); diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 16e2ea8ff..5f71defb9 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -152,7 +152,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { planning_scene::PlanningScenePtr end = start->diff(); const moveit::core::JointModelGroup* jmg = final_goal_state.getJointModelGroup(pair.first); final_goal_state.copyJointGroupPositions(jmg, positions); - robot_state::RobotState& goal_state = end->getCurrentStateNonConst(); + moveit::core::RobotState& goal_state = end->getCurrentStateNonConst(); goal_state.setJointGroupPositions(jmg, positions); goal_state.update(); intermediate_scenes.push_back(end); diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index 1d53ca4d2..dd820e9b8 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -150,7 +150,7 @@ void GenerateGraspPose::compute() { const std::string& eef = props.get("eef"); const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getEndEffector(eef); - robot_state::RobotState& robot_state = scene->getCurrentStateNonConst(); + moveit::core::RobotState& robot_state = scene->getCurrentStateNonConst(); try { applyPreGrasp(robot_state, jmg, props.property("pregrasp")); } catch (const moveit::Exception& e) { diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 2f5acb746..7af3d58c0 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -166,7 +166,7 @@ static void visualizePlan(std::deque& markers, Inter bool MoveRelative::compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& solution, Interface::Direction dir) { scene = state.scene()->diff(); - const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel(); + const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel(); assert(robot_model); const auto& props = properties(); @@ -282,7 +282,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints); if (robot_trajectory) { // the following requires a robot_trajectory returned from planning - robot_state::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr(); + moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr(); reached_state->updateLinkTransforms(); const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset; diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 0ce8ea116..c2721eedd 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -177,7 +177,7 @@ bool MoveTo::getPointGoal(const boost::any& goal, const Eigen::Isometry3d& ik_po bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& solution, Interface::Direction dir) { scene = state.scene()->diff(); - const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel(); + const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel(); assert(robot_model); const auto& props = properties(); diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index 8a3e875a9..c405499fc 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -120,7 +120,7 @@ void TaskDisplay::loadRobotModel() { const srdf::ModelSharedPtr& srdf = rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model()); - robot_model_.reset(new robot_model::RobotModel(rdf_loader_->getURDF(), srdf)); + robot_model_.reset(new moveit::core::RobotModel(rdf_loader_->getURDF(), srdf)); // Send to child class trajectory_visual_->onRobotModelLoaded(robot_model_); diff --git a/visualization/visualization_tools/src/display_solution.cpp b/visualization/visualization_tools/src/display_solution.cpp index 3d5dee7a7..bfb704c3b 100644 --- a/visualization/visualization_tools/src/display_solution.cpp +++ b/visualization/visualization_tools/src/display_solution.cpp @@ -65,7 +65,7 @@ float DisplaySolution::getWayPointDurationFromPrevious(const IndexPair& idx_pair return data_[idx_pair.first].trajectory_->getWayPointDurationFromPrevious(idx_pair.second); } -const robot_state::RobotStatePtr& DisplaySolution::getWayPointPtr(const IndexPair& idx_pair) const { +const moveit::core::RobotStatePtr& DisplaySolution::getWayPointPtr(const IndexPair& idx_pair) const { return data_[idx_pair.first].trajectory_->getWayPointPtr(idx_pair.second); } diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index f522f3967..b62c7e287 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -209,7 +209,7 @@ void TaskSolutionVisualization::setName(const QString& name) { slider_dock_panel_->setWindowTitle(name + " - Slider"); } -void TaskSolutionVisualization::onRobotModelLoaded(const robot_model::RobotModelConstPtr& robot_model) { +void TaskSolutionVisualization::onRobotModelLoaded(const moveit::core::RobotModelConstPtr& robot_model) { // Error check if (!robot_model) { ROS_ERROR_STREAM_NAMED("task_solution_visualization", "No robot model found");