moveit_sim_hw_interface.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2015, 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 the Univ of CO, Boulder 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 <dave@dav.ee>
36  Desc: Simulates a robot using ros_control controllers with a default position loaded from MoveIt!
37 */
38 
40 
41 // ROS parameter loading
43 
44 namespace moveit_sim_controller
45 {
47  : ros_control_boilerplate::SimHWInterface(nh, urdf_model), name_("moveit_sim_hw_interface")
48 {
49  // Load rosparams
50  ros::NodeHandle rpnh(nh_, name_);
51  std::size_t error = 0;
52  error += !rosparam_shortcuts::get(name_, rpnh, "joint_model_group", joint_model_group_);
53  error += !rosparam_shortcuts::get(name_, rpnh, "joint_model_group_pose", joint_model_group_pose_);
55 }
56 
58 {
59  // Call parent class version of this function
60  SimHWInterface::init();
61 
62  // Load the loader
64 
65  // Load default joint values
67 
68  ROS_INFO_STREAM_NAMED(name_, "MoveItSimHWInterface Ready.");
69 }
70 
72 {
73  // Load the robot model
74  robot_model::RobotModelPtr robot_model = robot_model_loader_->getModel(); // Get a shared pointer to the robot
75 
76  // Check for existance of joint model group
77  if (!robot_model->hasJointModelGroup(joint_model_group_))
78  {
79  ROS_WARN_STREAM_NAMED(name_, "Unable to find joint model group '" << joint_model_group_
80  << "' for the fake controller manager");
81  return;
82  }
83 
84  moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(joint_model_group_);
85 
86  // Load a robot state
87  moveit::core::RobotState robot_state(robot_model);
88 
89  // First set whole robot default values to ensure there are no 'nan's
90  robot_state.setToDefaultValues();
91 
92  // Attempt to set pose
93  if (!robot_state.setToDefaultValues(jmg, joint_model_group_pose_))
94  {
95  ROS_WARN_STREAM_NAMED(name_, "Unable to find pose " << joint_model_group_pose_ << " for the fake controller "
96  "manager");
97  return;
98  }
99  ROS_INFO_STREAM_NAMED(name_, "Set joints to pose " << joint_model_group_pose_);
100 
101  for (std::size_t i = 0; i < joint_names_.size(); ++i)
102  {
103  const moveit::core::JointModel* jm = robot_state.getJointModel(joint_names_[i]);
104 
105  // Error check
106  if (!jm)
107  {
108  ROS_WARN_STREAM_NAMED(name_, "Unable to find joint model group: " << joint_names_[i]);
109  continue;
110  }
111  if (jm->getVariableCount() != 1)
112  {
113  ROS_WARN_STREAM_NAMED(name_, "Fake joint controller does not currently accept more than 1 "
114  "variable per joint");
115  continue;
116  }
117 
118  // Set position from SRDF
119  joint_position_[i] = robot_state.getJointPositions(jm)[0];
121 
122  if (std::isnan(joint_position_[i]))
123  {
124  ROS_ERROR_STREAM_NAMED(name_, "NaN found");
125  std::cout << std::endl;
126  std::cout << "i: " << i << " name: " << jm->getName() << std::endl;
127  std::cout << "joint_model_group: " << jmg->getName() << std::endl;
128  std::cout << "getVariableCount(): " << jm->getVariableCount() << std::endl;
129  std::cout << "joint_position_[i]: " << joint_position_[i] << std::endl;
130  std::cout << "joint_position_command_[i]: " << joint_position_command_[i] << std::endl;
131  robot_state.printStateInfo();
132  exit(-1);
133  }
134  }
135 }
136 
137 } // namespace moveit_sim_controller
const std::string & getName() const
std::size_t getVariableCount() const
#define ROS_ERROR_STREAM_NAMED(name, args)
const std::string & getName() const
#define ROS_INFO_STREAM_NAMED(name, args)
void init()
Initialize the robot hardware interface.
robot_model_loader::RobotModelLoaderPtr robot_model_loader_
std::vector< std::string > joint_names_
void printStateInfo(std::ostream &out=std::cout) const
MoveItSimHWInterface(ros::NodeHandle &nh, urdf::Model *urdf_model=NULL)
Constructor.
const JointModel * getJointModel(const std::string &joint) const
void shutdownIfError(const std::string &parent_name, std::size_t error_count)
const double * getJointPositions(const std::string &joint_name) const
bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, bool &value)
static const std::string ROBOT_DESCRIPTION
#define ROS_WARN_STREAM_NAMED(name, args)


moveit_sim_controller
Author(s): Dave Coleman
autogenerated on Thu Jun 6 2019 19:38:57