robot_information.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2018, Sebastian Pütz
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions
6  * are met:
7  *
8  * 1. Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  *
11  * 2. Redistributions in binary form must reproduce the above
12  * copyright notice, this list of conditions and the following
13  * disclaimer in the documentation and/or other materials provided
14  * with the distribution.
15  *
16  * 3. Neither the name of the copyright holder nor the names of its
17  * contributors may be used to endorse or promote products derived
18  * from this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *
33  * robot_information.cpp
34  *
35  * author: Sebastian Pütz <spuetz@uni-osnabrueck.de>
36  *
37  */
38 
41 
42 namespace mbf_abstract_nav
43 {
44 
46  const std::string &global_frame,
47  const std::string &robot_frame,
48  const ros::Duration &tf_timeout)
49  : tf_listener_(tf_listener), global_frame_(global_frame), robot_frame_(robot_frame), tf_timeout_(tf_timeout)
50 {
51 
52 }
53 
54 
55 bool RobotInformation::getRobotPose(geometry_msgs::PoseStamped &robot_pose) const
56 {
58  ros::Duration(tf_timeout_), robot_pose);
59  robot_pose.header.stamp = ros::Time::now(); // would be 0 if not, as we ask tf listener for the last pose available
60  if (!tf_success)
61  {
62  ROS_ERROR_STREAM("Can not get the robot pose in the global frame. - robot frame: \""
63  << robot_frame_ << "\" global frame: \"" << global_frame_ << std::endl);
64  return false;
65  }
66  return true;
67 }
68 
69 bool RobotInformation::getRobotVelocity(geometry_msgs::TwistStamped &robot_velocity, ros::Duration look_back_duration) const
70 {
71  // TODO implement and filter tf data to compute velocity.
72  return false;
73 }
74 
75 const std::string& RobotInformation::getGlobalFrame() const {return global_frame_;};
76 
77 const std::string& RobotInformation::getRobotFrame() const {return robot_frame_;};
78 
80 
82 
83 } /* namespace mbf_abstract_nav */
const ros::Duration & getTfTimeout() const
const std::string & getRobotFrame() const
bool getRobotPose(const TF &tf, const std::string &robot_frame, const std::string &global_frame, const ros::Duration &timeout, geometry_msgs::PoseStamped &robot_pose)
bool getRobotPose(geometry_msgs::PoseStamped &robot_pose) const
Computes the current robot pose (robot_frame_) in the global frame (global_frame_).
RobotInformation(TF &tf_listener, const std::string &global_frame, const std::string &robot_frame, const ros::Duration &tf_timeout)
static Time now()
const std::string & getGlobalFrame() const
#define ROS_ERROR_STREAM(args)
bool getRobotVelocity(geometry_msgs::TwistStamped &robot_velocity, ros::Duration look_back_duration) const


mbf_abstract_nav
Author(s): Sebastian Pütz
autogenerated on Wed May 27 2020 04:03:14