send_lots_of_points_node.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011, Willow Garage, Inc.
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 Willow Garage, Inc. 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 #include "sensor_msgs/PointCloud.h"
32 #include "math.h"
33 
34 int main(int argc, char** argv)
35 {
36  ros::init(argc, argv, "send_lots_of_points");
37 
38  int rate = 1;
39  bool moving = true;
40  int size = 100;
41 
42  if (argc > 1)
43  {
44  rate = atoi(argv[1]);
45  }
46  if (argc > 2)
47  {
48  moving = bool(atoi(argv[2]));
49  }
50  if (argc > 3)
51  {
52  size = atoi(argv[3]);
53  }
54 
55  ros::NodeHandle nh;
56 
57  ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud>("lots_of_points", 100);
58  ros::Rate loop_rate(rate);
59 
60  sensor_msgs::PointCloud msg;
61  int width = size;
62  int length = 2 * size;
63  msg.points.resize(width * length);
64  msg.header.frame_id = "base_link";
65 
66  int count = 0;
67  while (ros::ok())
68  {
69  width++;
70  msg.points.resize(width * length + (count % 2));
71 
72  for (int x = 0; x < width; x++)
73  {
74  for (int y = 0; y < length; y++)
75  {
76  geometry_msgs::Point32& point = msg.points[x + y * width];
77  point.x = float(x / 100.0);
78  point.y = float(y / 100.0);
79  // point.z = sinf( x / 100.0 + y / 100.0 + count / 100.0 );
80  point.z = ((x + y + count) % 100) / 100.0;
81  }
82  }
83  if (count % 2)
84  {
85  msg.points[width * length + 1].x = -.1;
86  msg.points[width * length + 1].y = -.1;
87  msg.points[width * length + 1].z = 1.1;
88  }
89  msg.header.seq = count;
90  msg.header.stamp = ros::Time::now();
91 
92  printf("publishing at %d hz, %s, %d x %d points.\n", rate, (moving ? "moving" : "static"), width,
93  length);
94 
95  pub.publish(msg);
96 
97  ros::spinOnce();
98  loop_rate.sleep();
99  if (moving)
100  {
101  ++count;
102  }
103  }
104 }
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL bool ok()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
int main(int argc, char **argv)
static Time now()
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
ROSCPP_DECL void spinOnce()


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Fri Aug 12 2022 02:06:09