pr2_moveit_sensor_manager.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
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 Willow Garage 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: Ioan Sucan */
36 
37 #include <ros/ros.h>
40 #include <boost/math/constants/constants.hpp>
41 
43 #include <pr2_controllers_msgs/PointHeadAction.h>
44 
46 {
47 
49 {
50 public:
51 
53  {
54  node_handle_.param("head_pointing_frame", head_pointing_frame_, std::string("/camera"));
55  head_action_client_.reset(new actionlib::SimpleActionClient<pr2_controllers_msgs::PointHeadAction>("/head_traj_controller/point_head_action", true));
56  }
57 
59  {
60  }
61 
62  virtual void getSensorsList(std::vector<std::string> &names) const
63  {
64  names.resize(1);
65  names[0] = "head";
66  }
67 
68  virtual moveit_sensor_manager::SensorInfo getSensorInfo(const std::string &name) const
69  {
70  // I made this up.
72  if (name == "head")
73  {
75  si.min_dist = 0.1;
76  si.max_dist = 3.0;
77  si.x_angle = boost::math::constants::pi<double>() / 3.0;
78  si.y_angle = boost::math::constants::pi<double>() / 3.0;
79  }
80  else
81  ROS_ERROR("Unknown sensor: '%s'", name.c_str());
82  return si;
83  }
84 
85  virtual bool hasSensors() const
86  {
87  return true;
88  }
89 
90  virtual bool pointSensorTo(const std::string &name, const geometry_msgs::PointStamped &target, moveit_msgs::RobotTrajectory &sensor_trajectory)
91  {
92  if (name != "head")
93  {
94  ROS_ERROR("Unknown sensor: '%s'", name.c_str());
95  return false;
96  }
97  if (!head_action_client_->isServerConnected())
98  {
99  ROS_ERROR("Head action server is not connected");
100  return false;
101  }
102 
103  sensor_trajectory = moveit_msgs::RobotTrajectory();
104 
105  pr2_controllers_msgs::PointHeadGoal goal;
106  goal.pointing_frame = head_pointing_frame_;
107  goal.max_velocity = 1.0;
108  goal.pointing_axis.x = 0.0;
109  goal.pointing_axis.y = 0.0;
110  goal.pointing_axis.z = 1.0;
111  goal.target = target;
112  actionlib::SimpleClientGoalState result = head_action_client_->sendGoalAndWait(goal, ros::Duration(5.0));
114  return true;
115  else
116  {
117  ROS_WARN_STREAM("Head moving action is done with state " << result.toString() << ": " << result.getText());
118  return false;
119  }
120  }
121 
122 protected:
123 
125  std::string head_pointing_frame_;
127 
128 };
129 
132 }
virtual void getSensorsList(std::vector< std::string > &names) const
PLUGINLIB_EXPORT_CLASS(pr2_moveit_sensor_manager::Pr2MoveItSensorManager, moveit_sensor_manager::MoveItSensorManager)
std::string toString() const
virtual bool pointSensorTo(const std::string &name, const geometry_msgs::PointStamped &target, moveit_msgs::RobotTrajectory &sensor_trajectory)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
boost::shared_ptr< actionlib::SimpleActionClient< pr2_controllers_msgs::PointHeadAction > > head_action_client_
std::string getText() const
#define ROS_WARN_STREAM(args)
virtual moveit_sensor_manager::SensorInfo getSensorInfo(const std::string &name) const
#define ROS_ERROR(...)


pr2_moveit_plugins
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sun Nov 17 2019 03:24:45