multi_robot_goal_handler.cpp
Go to the documentation of this file.
1 #include "ros/ros.h"
2 #include <random>
3 #include <algorithm>
4 #include <iostream>
5 #include <fstream>
6 
7 #include <ros/ros.h>
9 #include <boost/algorithm/string.hpp>
10 #include <boost/date_time/posix_time/posix_time.hpp>
11 #include "tuw_geometry/utils.h"
12 
13 
15  : n_ ( n ), n_param_ ( "~" ), counter_(0) {
16  n_param_.param<std::string> ( "file_name", file_name_, "/tmp/goals.txt" );
17 
18  n_param_.param<bool> ( "run_once", run_once_, "false" );
19  if (mode == READ) {
20  n_param_.param<double> ( "loop_rate", loop_rate_, 1.0 );
21  n_param_.param<bool> ( "time_now", time_now_, "true" );
22  pub_goals_ = n.advertise<tuw_multi_robot_msgs::RobotGoalsArray> ( "goals", 1 );
23  }
24  if (mode == WRITE) {
25  sub_goals_ = n.subscribe ( "goals", 1, &GoalHandlerNode::callback, this );
26  }
27 }
28 
29 
30 void GoalHandlerNode::callback ( const tuw_multi_robot_msgs::RobotGoalsArray& msg ) {
31  ROS_INFO ( "GoalHandlerNode::callback" );
32  msg_ = msg;
33 
34  std::string frame_id_;
35  std::ofstream file;
36  char file_name[0x1FF];
37  if(run_once_){
38  sprintf(file_name, "%s", file_name_.c_str());
39  } else {
40  sprintf(file_name, "%s%03i", file_name_.c_str(), counter_);
41  }
42  file.open ( file_name );
43  ROS_INFO ( "Write file %s", file_name );
44  file << "tuw_multi_robot_msgs::RobotGoalsArray @" << boost::posix_time::to_iso_extended_string ( ros::Time::now().toBoost() ) << std::endl;
45  file << "frame_id: " << msg_.header.frame_id << std::endl;
46  file << std::setprecision ( std::numeric_limits<float>::digits10 + 1 );
47  file << std::setw ( 12 ) << msg_.header.stamp.sec << ", " << std::setw ( 12 ) << msg_.header.stamp.nsec << std::endl;
48  for ( size_t i = 0; i < msg_.robots.size(); i++ ) {
49  const tuw_multi_robot_msgs::RobotGoals &robot = msg_.robots[i];
50  file << robot.robot_name << std::endl;
51  for ( size_t j = 0; j < robot.destinations.size(); j++ ) {
52  const geometry_msgs::Pose p = robot.destinations[j];
53  file << std::setw ( 12 ) << p.position.x << ", " << std::setw ( 12 ) << p.position.y << ", " << std::setw ( 12 ) << p.position.z << ", ";
54  file << std::setw ( 12 ) << p.orientation.x << ", " << std::setw ( 12 ) << p.orientation.y << ", " << std::setw ( 12 ) << p.orientation.z << ", " << std::setw ( 12 ) << p.orientation.w << std::endl;
55  }
56  }
57  file.close();
58  counter_++;
60 }
62  ros::Rate loop_rate(loop_rate_);
63  do {
64  publishGoal();
65  ros::spinOnce();
66  loop_rate.sleep();
67  } while ( ros::ok() && (run_once_ == false));
68 }
69 
71  char file_name[0x1FF];
72  if(run_once_){
73  sprintf(file_name, "%s", file_name_.c_str());
74  } else {
75  sprintf(file_name, "%s%03i", file_name_.c_str(), counter_);
76  }
77 
78  std::ifstream file ( file_name );
79  if ( !file.good() ) {
80  ROS_ERROR ( "File: %s does not exist!", file_name );
81  run_once_ = true;
82  return;
83  }
84  if ( !file.is_open() ) {
85  ROS_ERROR ( "Can't open file %s!", file_name );
86  return;
87  }
88  ROS_INFO ( "Reading file %s", file_name );
89 
90  std::vector<std::string> columns;
91  std::string line;
92  // Titel
93  if ( getline ( file,line ) ) {
94  boost::erase_all ( line, " " );
95  boost::split ( columns, line, boost::is_any_of ( "@ " ) );
96  if ( ( columns.size() > 0 ) && boost::iequals ( columns[0],"tuw_multi_robot_msgs::RobotGoalsArray" ) ) {
97  ROS_DEBUG ( "Start reading tuw_multi_robot_msgs::RobotGoalsArray" );
98  } else {
99  ROS_ERROR ( "Missing keyword tuw_multi_robot_msgs::RobotGoalsArray" );
100  return;
101  }
102  } else {
103  ROS_ERROR ( "No header in File: %s!", file_name );
104  return;
105  }
106  // Header
107  if ( getline ( file,line ) ) {
108  boost::erase_all ( line, " " );
109  boost::split ( columns, line, boost::is_any_of ( ":" ) );
110  if ( ( columns.size() == 2 ) && boost::iequals ( columns[0],"frame_id" ) ) {
111  msg_.header.frame_id = columns[1];
112  } else {
113  ROS_ERROR ( "Missing keyword frame_id or frame_id value" );
114  }
115  } else {
116  ROS_ERROR ( "No header in File: %s!", file_name );
117  return;
118  }
119  // Timestamp
120  if ( getline ( file,line ) ) {
121  boost::erase_all ( line, " " );
122  boost::split ( columns, line, boost::is_any_of ( "," ) );
123  if ( columns.size() == 2 ) {
124  msg_.header.stamp.sec = std::stol ( columns[0] );
125  msg_.header.stamp.nsec = std::stol ( columns[1] );
126  } else {
127  ROS_INFO ( "Can't read timestamp" );
128  }
129  } else {
130  ROS_ERROR ( "No Timestamp in File: %s!", file_name );
131  return;
132  }
133  // Goals
134  tuw_multi_robot_msgs::RobotGoals::_destinations_type *destinations = NULL;
135  msg_.robots.clear();
136  while ( getline ( file,line ) ) {
137  boost::erase_all ( line, " " );
138  boost::split ( columns, line, boost::is_any_of ( "," ) );
139  // Robot name
140  if ( columns.size() == 1 ) {
141  if ( columns[0].size() > 0 ) {
142  tuw_multi_robot_msgs::RobotGoals robot;
143  robot.robot_name = line;
144  msg_.robots.push_back ( robot );
145  destinations = &msg_.robots.back().destinations;
146  } else {
147  ROS_ERROR ( "robot name to short!" );
148  return;
149  }
150  }
151  if ( ( columns.size() == 7 ) && ( destinations != NULL ) ) {
152  geometry_msgs::Pose p;
153  p.position.x = std::stod ( columns[0] );
154  p.position.y = std::stod ( columns[1] );
155  p.position.z = std::stod ( columns[2] );
156  p.orientation.x = std::stod ( columns[3] );
157  p.orientation.y = std::stod ( columns[4] );
158  p.orientation.z = std::stod ( columns[5] );
159  p.orientation.w = std::stod ( columns[6] );
160  destinations->push_back ( p );
161  }
162  if ( ( columns.size() == 4 ) && ( destinations != NULL ) ) {
163  geometry_msgs::Pose p;
164  p.position.x = std::stod ( columns[0] );
165  p.position.y = std::stod ( columns[1] );
166  p.position.z = std::stod ( columns[2] );
167  double alpha = std::stod ( columns[3] );
168  tuw::EulerYawToQuaternion ( alpha, p.orientation );
169  destinations->push_back ( p );
170  }
171  }
172  if(time_now_){
173  msg_.header.stamp = ros::Time::now();
174  }
175 
176  ros::Time timeout = ros::Time::now() + ros::Duration(10);
177  int nr_of_subscribers = pub_goals_.getNumSubscribers();
178  while (ros::Time::now() < timeout && (nr_of_subscribers <= 0)){
179  ROS_INFO ( "NumSubscribers: %i", nr_of_subscribers );
180  ros::Duration(0.1).sleep();
181  nr_of_subscribers = pub_goals_.getNumSubscribers();
182  }
183  ROS_INFO ( "NumSubscribers: %i", nr_of_subscribers );
184  pub_goals_.publish ( msg_ );
185 
186  file.close();
187  counter_++;
188 }
void publish(const boost::shared_ptr< M > &message) const
ros::Publisher pub_goals_
parameter
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool sleep() const
std::string file_name_
parameter
ros::Subscriber sub_goals_
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ROSCPP_DECL bool ok()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
tuw_multi_robot_msgs::RobotGoalsArray msg_
ros::NodeHandle n_param_
bool sleep()
uint32_t getNumSubscribers() const
void callback(const tuw_multi_robot_msgs::RobotGoalsArray &msg)
GoalHandlerNode(ros::NodeHandle &n, Mode mode)
static Time now()
ROSCPP_DECL void shutdown()
ROSCPP_DECL void spinOnce()
#define ROS_ERROR(...)
#define ROS_DEBUG(...)


tuw_multi_robot_goal_generator
Author(s):
autogenerated on Mon Jun 10 2019 15:42:34