octomap_server doesn't create map
I'm trying to let the octomap_server create a map from my Pointcloud2 messages. The pointcloud gets a transform, the header frame_id is correct and the header and the transform use the same timestamp.
But the octomap_server gives no output at all.
RViz visualizes the piontclouds correctly. Here is the code for generating the header and publishing the pointcloud/tf/clock.
self.pointcloud2_pub = rospy.Publisher('/sonar_pointcloud2', smsgs.PointCloud2, latch=True)
self.clock_pub = rospy.Publisher('/clock', rgmsgs.Clock, latch=True)
#Create Header
msg_header = stdmsgs.Header()
msg_header.seq = self.incId
self.incId += 1
msg_header.frame_id = "/sonar_pointcloud2"
msg_header.stamp = rospy.Time.now()
#.... filling the Pointcloud
#.... calculating new position and quaternion
#Transform Broadcaster
br.sendTransform((self.pose.x, self.pose.y, self.pose.z), (quat[0], quat[1], quat[2], quat[3]), msg_header.stamp, "/sonar_pointcloud2", "/map")
#Pointcloud2 Publisher
self.pointcloud2_pub.publish(msg_pointcloud2)
#Clock Publisher
self.clock_pub.publish(msg_header.stamp)
Here is my launch file for the octomap_server (credits to goetz.mark):
<!--
firing up the octomap server to generate 2D/3D occupancy maps
-->
<launch>
<node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
<!-- resolution in meters per pixel -->
<param name="resolution" value="0.1" />
<!-- name of the fixed frame, needs to be "/map" for SLAM -->
<param name="frame_id" type="string" value="/map" />
<!-- max range / depth resolution of the kinect in meter -->
<param name="sensor_model/max_range" value="50.0" />
<param name="latch" value="true" />
<!-- max/min height for occupancy map, should be in meters -->
<param name="pointcloud_max_z" value="100" />
<param name="pointcloud_min_z" value="100" />
<!-- topic from where pointcloud2 messages are subscribed -->
<remap from="cloud_in" to="/sonar_pointcloud2" />
</node>
</launch>
rqt_console error:
Nothing to publish, octree is empty
The node uses Groovy.
What am I doing wrong?
EDIT: The rqt_console error was due to a naming error in the launch file. I fixed this now, there are new errors now.
Is there a reason why you are explicitly publishing /clock and setting seq in the header? I haven't seen that in any publishing code before...
is there any other way to publish /tf topic. If I am using pointcloud from kinect.