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

Author: rhaschke

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

core/src/merge.cpp

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

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

core/src/solvers/pipeline_planner.cpp

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

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

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

core/src/stages/generate_grasp_pose.cpp

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

core/src/stages/move_relative.cpp

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

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

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