robot_info.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017, <copyright holder> <email>
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  * * Redistributions of source code must retain the above copyright
8  * notice, this list of conditions and the following disclaimer.
9  * * Redistributions in binary form must reproduce the above copyright
10  * notice, this list of conditions and the following disclaimer in the
11  * documentation and/or other materials provided with the distribution.
12  * * Neither the name of the <organization> nor the
13  * names of its contributors may be used to endorse or promote products
14  * derived from this software without specific prior written permission.
15  *
16  * THIS SOFTWARE IS PROVIDED BY <copyright holder> <email> ''AS IS'' AND ANY
17  * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19  * DISCLAIMED. IN NO EVENT SHALL <copyright holder> <email> BE LIABLE FOR ANY
20  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  *
27  */
28 #define POT_HIGH 1.0e10
29 
33 #include <tuw_multi_robot_msgs/Route.h>
34 #include <chrono>
35 #include <boost/functional/hash.hpp>
36 #include <boost/regex.hpp>
37 #include <tf/tf.h>
38 
39 //TODO add Weights from robots...
40 
41 
42 namespace multi_robot_router {
43 
44 
45 void RobotInfo::callback_odom ( const nav_msgs::Odometry &msg ) {
46  // ToDo
47 }
48 
49 bool RobotInfo::compareName(const std::shared_ptr<RobotInfo> robot) const{
50  std::string a = boost::regex_replace( this->robot_name, boost::regex("[^0-9]*([0-9]+).*"), std::string("\\1"));
51  std::string b = boost::regex_replace( robot->robot_name, boost::regex("[^0-9]*([0-9]+).*"), std::string("\\1"));
53  return std::stoi ( a ) < std::stoi ( b );
54 }
56 
57  //Not existant subscribe robots
58  std::string topic_odom_name = robot_name + "/odom";
59  ROS_DEBUG ( "Multi Robot Router: subscribing to %s", topic_odom_name.c_str() );
60  subOdom_ = n.subscribe ( topic_odom_name, 1, &RobotInfo::callback_odom, this );
61 
62  std::string topic_path_name = robot_name + "/path_unsynced";
63  ROS_DEBUG ( "Multi Robot Router: advertising on %s", topic_path_name.c_str() );
64  pubPaths_ = n.advertise<nav_msgs::Path> ( topic_path_name, 1, true );
65 
66 
67  std::string topic_route_name = robot_name + "/route";
68  ROS_DEBUG ( "Multi Robot Router: advertising on %s", topic_route_name.c_str() );
69  pubRoute_ = n.advertise<tuw_multi_robot_msgs::Route> ( topic_route_name, 1, true );
70 }
71 
72 void RobotInfo::updateInfo ( const tuw_multi_robot_msgs::RobotInfo &info ) {
73  tuw_multi_robot_msgs::RobotInfo &des = *this;
74  des = info;
75 }
76 
77 size_t RobotInfo::findIdx ( const std::vector<std::shared_ptr<RobotInfo> > &data, const std::string &name ) {
78  for ( size_t i = 0; i < data.size(); i++ ) {
79  if ( data[i]->robot_name == name ) return i;
80  }
81  return data.size();
82 }
83 
84 std::vector<std::shared_ptr<RobotInfo> >::iterator RobotInfo::findObj ( std::vector<std::shared_ptr<RobotInfo> > &data, const std::string &name ) {
85  return std::find_if ( data.begin(), data.end(), [&] ( auto & r ) {
86  return r->robot_name == name;
87  } );
88 }
89 
90 Eigen::Vector3d RobotInfo::getPose() const {
91  double roll, pitch, yaw;
92  tf::Quaternion q ( pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w );
93  tf::Matrix3x3 ( q ).getRPY ( roll, pitch, yaw );
94  Eigen::Vector3d p ( pose.pose.position.x, pose.pose.position.y, yaw );
95  return p;
96 }
97 
98 float RobotInfo::radius() const {
99  if ( shape == SHAPE_CIRCLE ) {
100  return shape_variables[0];
101  }
102 
103  return -1;
104 }
105 
107  return online_;
108 }
109 
110 void RobotInfo::updateOnlineStatus ( const float _updateTime ) {
111  if ( activeTime_ > 0 )
112  activeTime_ -= _updateTime;
113 
114  if ( activeTime_ < 0 )
115  activeTime_ = 0;
116 
117  if ( activeTime_ == 0 && online_ != Online::fixed )
119 }
120 
121 } // namespace multi_robot_router
static std::vector< std::shared_ptr< RobotInfo > >::iterator findObj(std::vector< std::shared_ptr< RobotInfo > > &data, const std::string &name)
Definition: robot_info.cpp:84
void updateOnlineStatus(const float updateTime)
Definition: robot_info.cpp:110
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
Eigen::Vector3d getPose() const
Definition: robot_info.cpp:90
static size_t findIdx(const std::vector< std::shared_ptr< RobotInfo > > &data, const std::string &name)
Definition: robot_info.cpp:77
Online getOnlineStatus() const
Definition: robot_info.cpp:106
bool compareName(const std::shared_ptr< RobotInfo > data) const
Definition: robot_info.cpp:49
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void callback_odom(const nav_msgs::Odometry &msg)
Definition: robot_info.cpp:45
void initTopics(ros::NodeHandle &n)
Definition: robot_info.cpp:55
std::queue< Index > q
#define ROS_DEBUG(...)
void updateInfo(const tuw_multi_robot_msgs::RobotInfo &info)
Definition: robot_info.cpp:72


tuw_multi_robot_router
Author(s): Benjamin Binder
autogenerated on Mon Jun 10 2019 15:42:49