imarker_robot_state.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2016, University of Colorado, Boulder
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of PickNik LLC nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Dave Coleman
36  Desc: Class to encapsule a visualized robot state that can be controlled using an interactive marker
37 */
38 
39 // MoveIt
42 
43 // this package
46 
47 // C++
48 #include <string>
49 #include <vector>
50 
51 namespace moveit_visual_tools
52 {
53 IMarkerRobotState::IMarkerRobotState(planning_scene_monitor::PlanningSceneMonitorPtr psm,
54  const std::string& imarker_name, std::vector<ArmData> arm_datas,
55  rviz_visual_tools::colors color, const std::string& package_path)
56  : name_(imarker_name), nh_("~"), psm_(psm), arm_datas_(arm_datas), color_(color), package_path_(package_path)
57 {
58  // Load Visual tools
59  visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(
60  psm_->getRobotModel()->getModelFrame(), nh_.getNamespace() + "/" + imarker_name, psm_);
61 
62  // visual_tools_->setPlanningSceneMonitor(psm_);
63  visual_tools_->loadRobotStatePub(nh_.getNamespace() + "/imarker_" + imarker_name + "_state");
64 
65  // Load robot state
66  imarker_state_ = std::make_shared<moveit::core::RobotState>(psm_->getRobotModel());
67  imarker_state_->setToDefaultValues();
68 
69  // Create Marker Server
70  const std::string imarker_topic = nh_.getNamespace() + "/" + imarker_name + "_imarker";
71  imarker_server_ = std::make_shared<interactive_markers::InteractiveMarkerServer>(imarker_topic, "", false);
72 
73  // Get file name
74  if (!getFilePath(file_path_, "imarker_" + name_ + ".csv", "config/imarkers"))
75  exit(-1);
76 
77  // Load previous pose from file
79  ROS_INFO_STREAM_NAMED(name_, "Unable to find state from file, setting to default");
80 
81  // Show initial robot state loaded from file
83 
84  // Create each end effector
85  end_effectors_.resize(arm_datas_.size());
86  for (std::size_t i = 0; i < arm_datas_.size(); ++i)
87  {
88  std::string eef_name;
89  if (i == 0)
90  eef_name = imarker_name + "_right";
91  else
92  eef_name = imarker_name + "_left";
93 
94  end_effectors_[i] = std::make_shared<IMarkerEndEffector>(this, eef_name, arm_datas_[i], color);
95 
96  // Create map from eef name to object
97  name_to_eef_[eef_name] = end_effectors_[i];
98  }
99 
100  // After both end effectors have been added, apply on server
101  imarker_server_->applyChanges();
102 
103  ROS_DEBUG_STREAM_NAMED(name_, "IMarkerRobotState '" << name_ << "' Ready.");
104 }
105 
106 bool IMarkerRobotState::loadFromFile(const std::string& file_name)
107 {
108  if (!boost::filesystem::exists(file_name))
109  {
110  ROS_WARN_STREAM_NAMED(name_, "File not found: " << file_name);
111  return false;
112  }
113  std::ifstream input_file(file_name);
114 
115  std::string line;
116 
117  if (!std::getline(input_file, line))
118  {
119  ROS_ERROR_STREAM_NAMED(name_, "Unable to read line");
120  return false;
121  }
122 
123  // Get robot state from file
125 
126  return true;
127 }
128 
130 {
131  output_file_.open(file_path_);
133  output_file_.close();
134 
135  return true;
136 }
137 
139 {
140  for (const IMarkerEndEffectorPtr ee : end_effectors_)
141  ee->setIMarkerCallback(callback);
142 }
143 
144 void IMarkerRobotState::setRobotState(moveit::core::RobotStatePtr state)
145 {
146  // Do a copy
147  *imarker_state_ = *state;
148 
149  // Update the imarkers
151  ee->setPoseFromRobotState();
152 }
153 
155 {
156  // Get the real current state
157  planning_scene_monitor::LockedPlanningSceneRO scene(psm_); // Lock planning scene
158  (*imarker_state_) = scene->getCurrentState();
159 
160  // Set updated pose from robot state
161  for (std::size_t i = 0; i < arm_datas_.size(); ++i)
162  end_effectors_[i]->setPoseFromRobotState();
163 
164  // Show new state
165  visual_tools_->publishRobotState(imarker_state_, color_);
166 }
167 
169 {
170  static const std::size_t MAX_ATTEMPTS = 1000;
171  for (std::size_t attempt = 0; attempt < MAX_ATTEMPTS; ++attempt)
172  {
173  // Set each planning group to random
174  for (std::size_t i = 0; i < arm_datas_.size(); ++i)
175  {
176  imarker_state_->setToRandomPositions(arm_datas_[i].jmg_);
177  }
178 
179  // Update transforms
180  imarker_state_->update();
182 
183  // Collision check
184  // which planning group to collision check, "" is everything
185  static const bool verbose = false;
186  if (planning_scene->isStateValid(*imarker_state_, "", verbose))
187  {
188  // Check clearance
189  if (clearance > 0)
190  {
191  // which planning group to collision check, "" is everything
192  if (planning_scene->distanceToCollision(*imarker_state_) < clearance)
193  {
194  continue; // clearance is not enough
195  }
196  }
197 
198  ROS_INFO_STREAM_NAMED(name_, "Found valid random robot state after " << attempt << " attempts");
199 
200  // Set updated pose from robot state
201  for (std::size_t i = 0; i < arm_datas_.size(); ++i)
202  end_effectors_[i]->setPoseFromRobotState();
203 
204  // Send to imarker
205  for (std::size_t i = 0; i < arm_datas_.size(); ++i)
206  end_effectors_[i]->sendUpdatedIMarkerPose();
207 
208  return true;
209  }
210 
211  if (attempt == 100)
212  ROS_WARN_STREAM_NAMED(name_, "Taking long time to find valid random state");
213  }
214 
215  ROS_ERROR_STREAM_NAMED(name_, "Unable to find valid random robot state for imarker after " << MAX_ATTEMPTS << " attem"
216  "pts");
217 
218  return false;
219 }
220 
222 {
223  // Update transforms
224  imarker_state_->update();
225 
227 
228  // which planning group to collision check, "" is everything
229  if (planning_scene->isStateValid(*imarker_state_, "", verbose))
230  {
231  return true;
232  }
233 
234  return false;
235 }
236 
238 {
239  visual_tools_->publishRobotState(imarker_state_, color_);
240 }
241 
243 {
244  return visual_tools_;
245 }
246 
247 bool IMarkerRobotState::getFilePath(std::string& file_path, const std::string& file_name,
248  const std::string& subdirectory) const
249 
250 {
251  namespace fs = boost::filesystem;
252 
253  // Check that the directory exists, if not, create it
254  fs::path rootPath = fs::path(package_path_);
255  rootPath = rootPath / fs::path(subdirectory);
256 
257  boost::system::error_code returnedError;
258  fs::create_directories(rootPath, returnedError);
259 
260  if (returnedError)
261  {
262  // did not successfully create directories
263  ROS_ERROR("Unable to create directory %s", subdirectory.c_str());
264  return false;
265  }
266 
267  // directories successfully created, append the group name as the file name
268  rootPath = rootPath / fs::path(file_name);
269  file_path = rootPath.string();
270  // ROS_DEBUG_STREAM_NAMED(name_, "Config file: " << file_path);
271 
272  return true;
273 }
274 
276 {
277  std::vector<std::string> tips;
278  for (std::size_t i = 0; i < arm_datas_.size(); ++i)
279  tips.push_back(arm_datas_[i].ee_link_->getName());
280 
281  // ROS_DEBUG_STREAM_NAMED(name_, "First pose should be for joint model group: " << arm_datas_[0].ee_link_->getName());
282 
283  const std::size_t attempts = 10;
284  const double timeout = 1.0 / 30.0; // 30 fps
285 
286  // Optionally collision check
288  if (true)
289  {
290  bool collision_checking_verbose_ = false;
291  bool only_check_self_collision_ = false;
292 
293  // TODO(davetcoleman): this is currently not working, the locking seems to cause segfaults
294  // TODO(davetcoleman): change to std shared_ptr
295  boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls;
297  constraint_fn = boost::bind(&isIKStateValid, static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls).get(),
298  collision_checking_verbose_, only_check_self_collision_, visual_tools_, _1, _2, _3);
299  }
300 
301  // Solve
302  std::size_t outer_attempts = 20;
303  for (std::size_t i = 0; i < outer_attempts; ++i)
304  {
305  if (!imarker_state_->setFromIK(group, poses, tips, attempts, timeout, constraint_fn))
306  {
307  ROS_DEBUG_STREAM_NAMED(name_, "Failed to find dual arm pose, trying again");
308 
309  // Re-seed
310  imarker_state_->setToRandomPositions(group);
311  }
312  else
313  {
314  ROS_DEBUG_STREAM_NAMED(name_, "Found IK solution");
315 
316  // Visualize robot
318 
319  // Update the imarkers
321  ee->setPoseFromRobotState();
322 
323  return true;
324  }
325  }
326 
327  ROS_ERROR_STREAM_NAMED(name_, "Failed to find dual arm pose");
328  return false;
329 }
330 
331 } // namespace moveit_visual_tools
332 
333 namespace
334 {
335 bool isIKStateValid(const planning_scene::PlanningScene* planning_scene, bool verbose, bool only_check_self_collision,
337  const moveit::core::JointModelGroup* group, const double* ik_solution)
338 {
339  // Apply IK solution to robot state
340  robot_state->setJointGroupPositions(group, ik_solution);
341  robot_state->update();
342 
343  // Ensure there are objects in the planning scene
344  if (false)
345  {
346  const std::size_t num_collision_objects = planning_scene->getCollisionWorld()->getWorld()->size();
347  if (num_collision_objects == 0)
348  {
349  ROS_ERROR_STREAM_NAMED("imarker_robot_state", "No collision objects exist in world, you need at least a table "
350  "modeled for the controller to work");
351  ROS_ERROR_STREAM_NAMED("imarker_robot_state", "To fix this, relaunch the teleop/head tracking/whatever MoveIt! "
352  "node to publish the collision objects");
353  return false;
354  }
355  }
356 
357  if (!planning_scene)
358  {
359  ROS_ERROR_STREAM_NAMED("imarker_robot_state", "No planning scene provided");
360  return false;
361  }
362  if (only_check_self_collision)
363  {
364  // No easy API exists for only checking self-collision, so we do it here.
365  // TODO(davetcoleman): move this into planning_scene.cpp
367  req.verbose = verbose;
368  req.group_name = group->getName();
370  planning_scene->checkSelfCollision(req, res, *robot_state);
371  if (!res.collision)
372  return true; // not in collision
373  }
374  else if (!planning_scene->isStateColliding(*robot_state, group->getName()))
375  return true; // not in collision
376 
377  // Display more info about the collision
378  if (verbose)
379  {
380  visual_tools->publishRobotState(*robot_state, rviz_visual_tools::RED);
381  planning_scene->isStateColliding(*robot_state, group->getName(), true);
382  visual_tools->publishContactPoints(*robot_state, planning_scene);
383  ROS_WARN_STREAM_THROTTLE_NAMED(2.0, "imarker_robot_state", "Collision in IK CC callback");
384  }
385 
386  return false;
387 }
388 
389 } // end annonymous namespace
IMarkerRobotState(planning_scene_monitor::PlanningSceneMonitorPtr psm, const std::string &imarker_name, std::vector< ArmData > arm_datas, rviz_visual_tools::colors color, const std::string &package_path)
Constructor.
void setToCurrentState()
Set the robot state to current in planning scene monitor.
#define ROS_DEBUG_STREAM_NAMED(name, args)
std::shared_ptr< MoveItVisualTools > MoveItVisualToolsPtr
#define ROS_ERROR_STREAM_NAMED(name, args)
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
std::function< void(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &, const Eigen::Affine3d &)> IMarkerCallback
bool setFromPoses(const EigenSTL::vector_Affine3d poses, const moveit::core::JointModelGroup *group)
void robotStateToStream(const RobotState &state, std::ostream &out, bool include_header=true, const std::string &separator=",")
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
const std::string & getName() const
std::vector< IMarkerEndEffectorPtr > end_effectors_
bool isStateValid(bool verbose=false)
Return true if the currently solved IK solution is valid.
void publishRobotState()
Show current state in Rviz.
#define ROS_INFO_STREAM_NAMED(name, args)
bool setToRandomState(double clearance=0)
Set the robot to a random position.
std::shared_ptr< IMarkerEndEffector > IMarkerEndEffectorPtr
bool loadFromFile(const std::string &file_name)
InteractiveMarkerServerPtr imarker_server_
bool verbose
void update(bool force=false)
#define ROS_WARN_STREAM_THROTTLE_NAMED(period, name, args)
bool getFilePath(std::string &file_path, const std::string &file_name, const std::string &subdirectory) const
const std::string & getNamespace() const
std::map< std::string, IMarkerEndEffectorPtr > name_to_eef_
planning_scene_monitor::PlanningSceneMonitorPtr psm_
moveit::core::RobotStatePtr imarker_state_
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
void setRobotState(moveit::core::RobotStatePtr state)
Set the robot state.
moveit_visual_tools::MoveItVisualToolsPtr visual_tools_
void setIMarkerCallback(IMarkerCallback callback)
Set where in the parent class the feedback should be sent.
void streamToRobotState(RobotState &state, const std::string &line, const std::string &separator=",")
#define ROS_ERROR(...)
boost::function< bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn
moveit_visual_tools::MoveItVisualToolsPtr getVisualTools()
bool isStateColliding(const std::string &group="", bool verbose=false)
const collision_detection::CollisionWorldConstPtr & getCollisionWorld() const
#define ROS_WARN_STREAM_NAMED(name, args)


moveit_visual_tools
Author(s): Dave Coleman
autogenerated on Thu Jun 6 2019 19:40:28