9 #include <boost/algorithm/string.hpp> 10 #include <boost/date_time/posix_time/posix_time.hpp> 11 #include "tuw_geometry/utils.h" 15 : n_ ( n ), n_param_ (
"~" ), counter_(0) {
31 ROS_INFO (
"GoalHandlerNode::callback" );
34 std::string frame_id_;
36 char file_name[0x1FF];
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;
71 char file_name[0x1FF];
78 std::ifstream file ( file_name );
80 ROS_ERROR (
"File: %s does not exist!", file_name );
84 if ( !file.is_open() ) {
85 ROS_ERROR (
"Can't open file %s!", file_name );
88 ROS_INFO (
"Reading file %s", file_name );
90 std::vector<std::string> columns;
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" );
99 ROS_ERROR (
"Missing keyword tuw_multi_robot_msgs::RobotGoalsArray" );
103 ROS_ERROR (
"No header in File: %s!", file_name );
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];
113 ROS_ERROR (
"Missing keyword frame_id or frame_id value" );
116 ROS_ERROR (
"No header in File: %s!", file_name );
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] );
127 ROS_INFO (
"Can't read timestamp" );
130 ROS_ERROR (
"No Timestamp in File: %s!", file_name );
134 tuw_multi_robot_msgs::RobotGoals::_destinations_type *destinations = NULL;
136 while ( getline ( file,line ) ) {
137 boost::erase_all ( line,
" " );
138 boost::split ( columns, line, boost::is_any_of (
"," ) );
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;
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 );
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 );
179 ROS_INFO (
"NumSubscribers: %i", nr_of_subscribers );
183 ROS_INFO (
"NumSubscribers: %i", nr_of_subscribers );
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())
std::string file_name_
parameter
ros::Subscriber sub_goals_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
tuw_multi_robot_msgs::RobotGoalsArray msg_
uint32_t getNumSubscribers() const
void callback(const tuw_multi_robot_msgs::RobotGoalsArray &msg)
GoalHandlerNode(ros::NodeHandle &n, Mode mode)
ROSCPP_DECL void shutdown()
ROSCPP_DECL void spinOnce()