select_map.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2014-2017, the neonavigation authors
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  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <ros/ros.h>
31 
32 #include <map_organizer_msgs/OccupancyGridArray.h>
33 #include <std_msgs/Int32.h>
36 
37 #include <vector>
38 
40 
41 map_organizer_msgs::OccupancyGridArray maps;
42 std::vector<nav_msgs::MapMetaData> orig_mapinfos;
43 int floor_cur = 0;
44 
45 void cbMaps(const map_organizer_msgs::OccupancyGridArray::Ptr& msg)
46 {
47  ROS_INFO("Map array received");
48  maps = *msg;
49  orig_mapinfos.clear();
50  for (auto& map : maps.maps)
51  {
52  orig_mapinfos.push_back(map.info);
53  map.info.origin.position.z = 0.0;
54  }
55 }
56 void cbFloor(const std_msgs::Int32::Ptr& msg)
57 {
58  floor_cur = msg->data;
59 }
60 
61 int main(int argc, char** argv)
62 {
63  ros::init(argc, argv, "select_map");
64  ros::NodeHandle pnh("~");
65  ros::NodeHandle nh("");
66 
69  nh, "maps",
70  nh, "/maps", 1, cbMaps);
72  nh, "floor",
73  pnh, "floor", 1, cbFloor);
74  auto pubMap = neonavigation_common::compat::advertise<nav_msgs::OccupancyGrid>(
75  nh, "map",
76  nh, "/map", 1, true);
77 
79  geometry_msgs::TransformStamped trans;
80  trans.header.frame_id = "map_ground";
81  trans.child_frame_id = "map";
82  trans.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), 0.0));
83 
84  ros::Rate wait(10);
85  int floor_prev = -1;
86  while (ros::ok())
87  {
88  wait.sleep();
89  ros::spinOnce();
90 
91  if (maps.maps.size() == 0)
92  continue;
93 
94  if (floor_cur != floor_prev)
95  {
96  if (floor_cur >= 0 && floor_cur < static_cast<int>(maps.maps.size()))
97  {
98  pubMap.publish(maps.maps[floor_cur]);
99  trans.transform.translation.z = orig_mapinfos[floor_cur].origin.position.z;
100  }
101  else
102  {
103  ROS_INFO("Floor out of range");
104  }
105  floor_prev = floor_cur;
106  }
107  trans.header.stamp = ros::Time::now() + ros::Duration(0.15);
108  tfb.sendTransform(trans);
109  }
110 
111  return 0;
112 }
map_organizer_msgs::OccupancyGridArray maps
Definition: select_map.cpp:41
std::vector< nav_msgs::MapMetaData > orig_mapinfos
Definition: select_map.cpp:42
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void cbMaps(const map_organizer_msgs::OccupancyGridArray::Ptr &msg)
Definition: select_map.cpp:45
ros::Subscriber subscribe(ros::NodeHandle &nh_new, const std::string &topic_new, ros::NodeHandle &nh_old, const std::string &topic_old, uint32_t queue_size, void(*fp)(M), const ros::TransportHints &transport_hints=ros::TransportHints())
int main(int argc, char **argv)
Definition: select_map.cpp:61
#define ROS_INFO(...)
ROSCPP_DECL bool ok()
bool sleep()
B toMsg(const A &a)
void cbFloor(const std_msgs::Int32::Ptr &msg)
Definition: select_map.cpp:56
static Time now()
ROSCPP_DECL void spinOnce()
int floor_cur
Definition: select_map.cpp:43


map_organizer
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:20:33