47 #include <boost/filesystem.hpp>
59 const double* ik_solution)
65 #if 0 // Ensure there are objects in the planning scene
66 const std::size_t num_collision_objects =
planning_scene->getCollisionEnv()->getWorld()->size();
67 if (num_collision_objects == 0)
69 ROS_ERROR_STREAM_NAMED(
"imarker_robot_state",
"No collision objects exist in world, you need at least a table "
70 "modeled for the controller to work");
71 ROS_ERROR_STREAM_NAMED(
"imarker_robot_state",
"To fix this, relaunch the teleop/head tracking/whatever MoveIt "
72 "node to publish the collision objects");
82 if (only_check_self_collision)
114 const std::string& imarker_name, std::vector<ArmData> arm_datas,
116 : name_(imarker_name)
118 , arm_datas_(
std::move(arm_datas))
119 , psm_(
std::move(psm))
121 , package_path_(package_path)
124 visual_tools_ = std::allocate_shared<moveit_visual_tools::MoveItVisualTools>(
125 Eigen::aligned_allocator<moveit_visual_tools::MoveItVisualTools>(),
psm_->getRobotModel()->getModelFrame(),
136 const std::string imarker_topic =
nh_.
getNamespace() +
"/" + imarker_name +
"_imarker";
137 imarker_server_ = std::make_shared<interactive_markers::InteractiveMarkerServer>(imarker_topic,
"",
false);
152 for (std::size_t i = 0; i <
arm_datas_.size(); ++i)
154 std::string eef_name;
156 eef_name = imarker_name +
"_right";
158 eef_name = imarker_name +
"_left";
161 end_effectors_[i] = std::allocate_shared<IMarkerEndEffector>(Eigen::aligned_allocator<IMarkerEndEffector>(),
this,
176 if (!boost::filesystem::exists(file_name))
181 std::ifstream input_file(file_name);
185 if (!std::getline(input_file, line))
209 ee->setIMarkerCallback(callback);
219 ee->setPoseFromRobotState();
226 (*imarker_state_) = scene->getCurrentState();
229 for (std::size_t i = 0; i <
arm_datas_.size(); ++i)
238 static const std::size_t MAX_ATTEMPTS = 1000;
239 for (std::size_t attempt = 0; attempt < MAX_ATTEMPTS; ++attempt)
242 for (std::size_t i = 0; i <
arm_datas_.size(); ++i)
253 static const bool verbose =
false;
269 for (std::size_t i = 0; i <
arm_datas_.size(); ++i)
273 for (std::size_t i = 0; i <
arm_datas_.size(); ++i)
284 "Unable to find valid random robot state for imarker after " << MAX_ATTEMPTS <<
" attempts");
311 const std::string& subdirectory)
const
314 namespace fs = boost::filesystem;
318 rootPath = rootPath / fs::path(subdirectory);
320 boost::system::error_code returnedError;
321 fs::create_directories(rootPath, returnedError);
326 ROS_ERROR(
"Unable to create directory %s", subdirectory.c_str());
331 rootPath = rootPath / fs::path(file_name);
332 file_path = rootPath.string();
341 std::vector<std::string> tips;
342 for (std::size_t i = 0; i <
arm_datas_.size(); ++i)
343 tips.push_back(
arm_datas_[i].ee_link_->getName());
347 const double timeout = 1.0 / 30.0;
352 bool collision_checking_verbose_ =
false;
353 bool only_check_self_collision_ =
false;
357 boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls;
359 constraint_fn = boost::bind(&isIKStateValid,
static_cast<const planning_scene::PlanningSceneConstPtr&
>(*ls).get(),
360 collision_checking_verbose_, only_check_self_collision_,
visual_tools_,
361 boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3);
365 std::size_t outer_attempts = 20;
366 for (std::size_t i = 0; i < outer_attempts; ++i)
368 if (!
imarker_state_->setFromIK(group, poses, tips, timeout, constraint_fn))
384 ee->setPoseFromRobotState();