LocalizerNode.cpp
Go to the documentation of this file.
1 #include "ros/ros.h"
3 #include "nav_msgs/GetMap.h"
4 
6 
7 int main(int argc, char **argv)
8 {
9  ros::init(argc, argv, "localize_node");
11 
12  SelfLocalizer selfLocalizer(true);
13 
15 
16  // Subscribe to laser scans
17  std::string laserTopic;
18  n.param("laser_topic", laserTopic, std::string("scan"));
19  ros::Subscriber laserSubscriber = n.subscribe(laserTopic, 100, &SelfLocalizer::process, &selfLocalizer);
20 
21  // Get the map via service call
22  std::string mapService;
23  n.param("map_service", mapService, std::string("get_map"));
24  ros::ServiceClient mapClient = n.serviceClient<nav_msgs::GetMap>(mapService);
25 
26  nav_msgs::GetMap srv;
27  mapClient.waitForExistence();
28  if(mapClient.call(srv))
29  {
30  selfLocalizer.convertMap(srv.response.map);
31  selfLocalizer.initialize();
32  }else
33  {
34  ROS_FATAL("Could not get a map.");
35  return 1;
36  }
37 
38  ros::Rate loopRate(10);
39  while(ros::ok())
40  {
41  tfBC.sendTransform(selfLocalizer.getMapToOdometry());
42 
43  ros::spinOnce();
44  loopRate.sleep();
45  }
46 
47  ros::spin();
48  return 0;
49 }
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
#define ROS_FATAL(...)
void process(const sensor_msgs::LaserScan::ConstPtr &scan)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
tf::StampedTransform getMapToOdometry()
Definition: SelfLocalizer.h:56
int main(int argc, char **argv)
static int n
Definition: eig3.c:12
ROSCPP_DECL void spin(Spinner &spinner)
void convertMap(const nav_msgs::OccupancyGrid &map_msg)
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ROSCPP_DECL bool ok()
void sendTransform(const StampedTransform &transform)
bool sleep()
ROSCPP_DECL void spinOnce()


nav2d_localizer
Author(s): Sebastian Kasperski
autogenerated on Thu Jun 6 2019 19:20:19