move_demo_node.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
3 
4 using namespace grid_map;
5 using namespace ros;
6 
7 int main(int argc, char** argv)
8 {
9  // Initialize node and publisher.
10  init(argc, argv, "move_demo");
11  NodeHandle nh("~");
12  Publisher publisher = nh.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
13 
14  // Create grid map.
15  GridMap map({"layer"});
16  map.setFrameId("map");
17  map.setGeometry(Length(0.7, 0.7), 0.01, Position(0.0, 0.0));
18  ROS_INFO("Created map with size %f x %f m (%i x %i cells).\n The center of the map is located at (%f, %f) in the %s frame.",
19  map.getLength().x(), map.getLength().y(),
20  map.getSize()(0), map.getSize()(1),
21  map.getPosition().x(), map.getPosition().y(), map.getFrameId().c_str());
22  map["layer"].setRandom();
23 
24  bool useMoveMethod = true;
25  while (nh.ok()) {
26 
27  if (useMoveMethod) {
28  ROS_INFO("Using the `move(...)` method.");
29  } else {
30  ROS_INFO("Using the `setPosition(...)` method.");
31  }
32 
33  // Work with temporary map in a loop.
34  GridMap tempMap(map);
35  Rate rate(10.0);
36  ros::Time startTime = ros::Time::now();
38 
39  while (duration <= ros::Duration(10.0)) {
40  ros::Time time = ros::Time::now();
41  duration = time - startTime;
42 
43  // Change position of the map with either the `move` or `setPosition` method.
44  const double t = duration.toSec();
45  Position newPosition = 0.03 * t * Position(cos(t), sin(t));
46 
47  if (useMoveMethod) {
48  tempMap.move(newPosition);
49  } else {
50  tempMap.setPosition(newPosition);
51  }
52 
53  // Publish grid map.
54  tempMap.setTimestamp(time.toNSec());
55  grid_map_msgs::GridMap message;
56  GridMapRosConverter::toMessage(tempMap, message);
57  publisher.publish(message);
58  ROS_DEBUG("Grid map (duration %f) published with new position [%f, %f].",
59  duration.toSec(), tempMap.getPosition().x(), tempMap.getPosition().y());
60  rate.sleep();
61  }
62 
63  useMoveMethod = !useMoveMethod;
64  }
65 
66  return 0;
67 }
int main(int argc, char **argv)
void setPosition(const Position &position)
void publish(const boost::shared_ptr< M > &message) const
bool getPosition(const Index &index, Position &position) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
static void toMessage(const grid_map::GridMap &gridMap, grid_map_msgs::GridMap &message)
#define duration(a)
bool move(const Position &position, std::vector< BufferRegion > &newRegions)
geometry_msgs::TransformStamped t
Node for comparing different normal filters performances.
Eigen::Vector2d Position
#define ROS_INFO(...)
uint64_t toNSec() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool sleep()
static Time now()
bool ok() const
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
void setFrameId(const std::string &frameId)
void setTimestamp(const Time timestamp)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Eigen::Array2d Length
#define ROS_DEBUG(...)


grid_map_demos
Author(s): Péter Fankhauser
autogenerated on Tue Jun 1 2021 02:13:55