Replace namespace robot_[model|state] with moveit::core

This commit is contained in:
Robert Haschke 2023-05-26 00:09:21 +02:00 committed by Robert Haschke
parent 6f7282423d
commit c605a0059a
12 changed files with 27 additions and 27 deletions

View File

@ -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();

View File

@ -105,7 +105,7 @@ moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::Joint
robot_trajectory::RobotTrajectoryPtr
merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& 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<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
std::vector<double> values;
values.reserve(max_num_vars);
auto merged_state = std::make_shared<robot_state::RobotState>(base_state);
auto merged_state = std::make_shared<moveit::core::RobotState>(base_state);
while (true) {
bool finished = true;
size_t index = merged_traj->getWayPointCount();
@ -151,7 +151,7 @@ merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& 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<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
// add waypoint without timing
merged_traj->addSuffixWayPoint(merged_state, 0.0);
// create new RobotState for next waypoint
merged_state = std::make_shared<robot_state::RobotState>(*merged_state);
merged_state = std::make_shared<moveit::core::RobotState>(*merged_state);
}
// add timing

View File

@ -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<const robot_state::RobotState&>(*state), jmg->getName()) &&
return !sandbox_scene->isStateColliding(const_cast<const moveit::core::RobotState&>(*state), jmg->getName()) &&
kcs.decide(*state).satisfied;
};

View File

@ -52,10 +52,10 @@ struct PlannerCache
{
using PlannerID = std::tuple<std::string, std::string>;
using PlannerMap = std::map<PlannerID, std::weak_ptr<planning_pipeline::PlanningPipeline> >;
using ModelList = std::list<std::pair<std::weak_ptr<const robot_model::RobotModel>, PlannerMap> >;
using ModelList = std::list<std::pair<std::weak_ptr<const moveit::core::RobotModel>, 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()) {

View File

@ -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<double> compare_pose;
const std::string& compare_pose_name = props.get<std::string>("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();

View File

@ -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);

View File

@ -150,7 +150,7 @@ void GenerateGraspPose::compute() {
const std::string& eef = props.get<std::string>("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) {

View File

@ -166,7 +166,7 @@ static void visualizePlan(std::deque<visualization_msgs::Marker>& 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;

View File

@ -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();

View File

@ -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_);

View File

@ -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);
}

View File

@ -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");