save_image_bundle.cpp
Go to the documentation of this file.
1 
7 /*****************************************************************************
8 ** Includes
9 *****************************************************************************/
10 
12 #include <ecl/command_line.hpp>
13 #include <ecl/console.hpp>
14 #include <exception>
15 
16 #include <ros/init.h>
17 #include <ros/ros.h>
18 #include <string>
19 
20 #include "../../include/cost_map_ros/utilities.hpp"
21 
22 /*****************************************************************************
23 ** Main
24 *****************************************************************************/
25 
26 int main(int argc, char **argv) {
27  ecl::CmdLine cmd("Save a published cost map to an image bundle");
29  "topic_name",
30  "Topic name to listen to for costmaps (e.g. /image_bundle/cost_map)",
31  true,
32  "/image_bundle/cost_map",
33  "string");
35  "filename",
36  "Filename of the bundle's meta yaml file.",
37  true,
38  "foo.yaml",
39  "string");
40  cmd.add(topicNameArg);
41  cmd.add(filenameArg);
42 
43  std::vector<std::string> myargs;
44  ros::removeROSArgs(argc, argv, myargs);
45  char** myargv = new char*[myargs.size()]; //array to emulate argv.
46  for(int i = 0, ie = myargs.size(); i < ie; ++i)
47  {
48  myargv[i] = const_cast<char*>(myargs[i].data());
49  }
50  cmd.parse(myargs.size(), myargv);
51 
52  ros::init(argc, argv, "save_image_bundle");
53  try {
54  cost_map::SaveImageBundle dumper(topicNameArg.getValue(), filenameArg.getValue());
55 // if ( broken_layer_probably_isnt_obstacle_costs ) {
56 // cost_map::Matrix data = loader.cost_map->get("obstacle_costs");
57 // for (int i = 0, number_of_rows = data.rows(), number_of_columns = data.cols(); i < number_of_rows; ++i) {
58 // for (int j = 0; j < number_of_columns; ++j) {
59 // std::cout << static_cast<int>(data(i,j)) << " ";
60 // }
61 // std::cout << std::endl;
62 // }
63 // }
64  ros::Rate rate(10);
65  while ( ros::ok() ) {
66  ros::spinOnce();
67  if ( dumper.finished ) {
68  break;
69  } else {
70  ROS_WARN_STREAM_DELAYED_THROTTLE(5, "SaveImageBundle : still waiting for a costmap to arrive [zzzz....]");
71  rate.sleep();
72  }
73  }
74  } catch (const std::exception& e) {
75  std::cout << ecl::red << "[ERROR] " << e.what() << ecl::reset << std::endl;
76  return EXIT_FAILURE;
77  }
78  return EXIT_SUCCESS;
79 }
string cmd
int main(int argc, char **argv)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void add(Arg &a)
ROSCPP_DECL bool ok()
#define ROS_WARN_STREAM_DELAYED_THROTTLE(rate, args)
Helper for saving an image bundle from a cost map topic.
ROSCPP_DECL void removeROSArgs(int argc, const char *const *argv, V_string &args_out)
void parse(int argc, char **argv)
ROSCPP_DECL void spinOnce()


cost_map_ros
Author(s): Daniel Stonier
autogenerated on Mon Jun 10 2019 13:03:48