Ask Your Question
0

Segmentation fault while visualizing own PointCloud2 Message

asked 2013-06-19 03:29:12 -0500

Josch gravatar image

Hi everyone,

I wrote a python class which should convert an 2D-Array with intensities to a Pointcloud an publish it. Unfortunately RViz crashes, as soon as it should show something. The error is the following:

Segmentation fault (Memory image written) (Translation is hand done and may be vague)

Is there any tutorial how to correctly generate a PointCloud2 message in Python? If not, is somebody able to give me a simple short code block how to do it? I feel like I missed something....

This is the class description:

sensor_msgs/PointCloud2

Python conversion to bits

This is the python class: #!/usr/bin/env python

import sensor_msgs.msg as smsgs
import std_msgs.msg as stdmsgs
import math as m
import tf

POINTCLOUD_TOPIC = 'sonar'
NODE_NAME = 'adapter'

class Adapter():
    def receive(self):
        self.sonarToPointfield()

    def sonarToPointfield(self):

        #  Create Header
        msg_header = stdmsgs.Header()
        msg_header.seq = 1
        msg_header.frame_id = "/map"
        msg_header.stamp = rospy.Time.now()

        #  Create 4 PointFields as channel description
        msg_pf1 = smsgs.PointField()
        msg_pf1.name = 'x'
        msg_pf1.offset = 0
        msg_pf1.datatype = 7
        msg_pf1.count = 1

        msg_pf2 = smsgs.PointField()
        msg_pf2.name = 'y'
        msg_pf2.offset = 4
        msg_pf2.datatype = 7
        msg_pf2.count = 1

        msg_pf3 = smsgs.PointField()
        msg_pf3.name = 'z'
        msg_pf3.offset = 8
        msg_pf3.datatype = 7
        msg_pf3.count = 1

        msg_pf4 = smsgs.PointField()
        msg_pf4.name = 'intensity'
        msg_pf4.offset = 12
        msg_pf4.datatype = 2
        msg_pf4.count = 1

        resultList = [2.0, 3.0, 4.0, 1, 5.0, 6.0, 7.0, 2]
        resultList = [0, 0, 0, 200]

        #  len(resultList) / 4 is the number of points, since every point consists of 4 elements
        #  one point representation (x=FLOAT32, y=FLOAT32, z=FLOAT32, intensity=UINT8) is 13 bytes long
        resultList2 = []
        resultArray = np.zeros((len(resultList) / 4) * 13, dtype=np.ubyte)
        print "len(resultArray): ", len(resultArray)
        for x in range(0, len(resultList)-1, 4):
            tmpArr = np.zeros(13, np.ubyte)
            print "Coords: " + str(resultList[x+0]) + ", " + str(resultList[x+1]) + ", " + str(resultList[x+2]) + ", " + str(resultList[x+3])
            struct.pack_into('fffB', tmpArr, 0, resultList[x+0], resultList[x+1], resultList[x+2], resultList[x+3])
            print "tmpArr: ", tmpArr
            for i in range(len(tmpArr)):
                resultList2.append(tmpArr[i])
                resultArray[((x/4)*13)+i] = tmpArr[i]

        #  Create PointCloud2 instance
        msg_pointcloud2 = smsgs.PointCloud2()

        #  Fill message
        msg_pointcloud2.header = msg_header
        msg_pointcloud2.fields = [msg_pf1, msg_pf2, msg_pf3, msg_pf4]
        msg_pointcloud2.data = resultList2
        msg_pointcloud2.width = len(resultList2)
        msg_pointcloud2.height = 1
        msg_pointcloud2.point_step = 13
        msg_pointcloud2.row_step = 1560
        msg_pointcloud2.is_dense = False
        msg_pointcloud2.is_bigendian = True

        self.publishPointCloud2(msg_pointcloud2)

    def publishPointCloud2(self, msg_pointcloud2):
        self.pub.publish(msg_pointcloud2)
        self.publishWorldFrame()

    def publishWorldFrame(self):
        br = tf.TransformBroadcaster()
        br.sendTransform((0, 0, 0), (1, 1, 1, 1), rospy.Time.now(), "/sonar_pointcloud", "/map")

    def init(self):
        self.pub = rospy.Publisher('sonar_pointcloud', smsgs.PointCloud2)
        rospy.init_node('pcl_adapter')
        self.receive()

if __name__ == '__main__':
    adapter = Adapter()
    adapter.init()

Thank you in advance, Josch

edit retag flag offensive close merge delete

3 Answers

Sort by » oldest newest most voted
0

answered 2013-07-17 03:17:23 -0500

Josch gravatar image

The row step was calculated wrong, since I only tested on a simulated input. Using a dynamic row_step worked for me.

edit flag offensive delete link more
2

answered 2013-06-19 08:13:27 -0500

hersh gravatar image
edit flag offensive delete link more

Comments

Okay thanks. So there's nothing i can do about it right now?

Josch gravatar imageJosch ( 2013-06-20 23:22:35 -0500 )edit

After reviewing this, I think it's probably not a bug in RViz. Couldn't reproduce the crash after fixing the python script posted with the question.

dgossow gravatar imagedgossow ( 2013-06-21 07:31:19 -0500 )edit
0

answered 2013-06-21 06:37:11 -0500

dgossow gravatar image

updated 2013-06-25 08:27:41 -0500

The problem is not related to the point cloud but to the fact that you're immediately destroying your publisher and exit the program. Publishing in ROS means your node needs to connect to the master, advertise that it is publishing on a topic and the other ROS nodes need to create one-to-one tcp connections to your node.

So, two changes you have to make to your script to make this work:

  • add a rospy.spin() at the end of your main, so the program stays alive after publishing
  • use a latched publisher, so that when other nodes connect after a short while they will still receive the message you generate at startup: self.pub = rospy.Publisher('sonar_pointcloud', smsgs.PointCloud2, latch=True)
edit flag offensive delete link more

Comments

Thank you, i will try this as soon as I can.

Josch gravatar imageJosch ( 2013-06-21 06:42:58 -0500 )edit

Unfortunately RViz still crashes with the same error when it should draw a single point at (0,0,0). I tried both methods, the latched publisher and a rospy.spin.

Josch gravatar imageJosch ( 2013-06-24 22:11:40 -0500 )edit

And at random this error came up: rviz: /usr/include/boost/thread/pthread/pthread_mutex_scoped_lock.hpp:26: boost::pthread::pthread_mutex_scoped_lock::pthread_mutex_scoped_lock(pthread_mutex_t*): Assertion »!pthread_mutex_lock(m)« not complied.

Josch gravatar imageJosch ( 2013-06-24 22:14:40 -0500 )edit

Both changes are needed at the same time.

dgossow gravatar imagedgossow ( 2013-06-25 08:28:14 -0500 )edit

Unfortunately still the same. As soon as the Pointcloud is published, RViz crashes. When you try that code on your PC, you say it's working?

Josch gravatar imageJosch ( 2013-07-09 21:28:08 -0500 )edit

I figured out the problem was a misscalculation in the row size. It's working now!

Josch gravatar imageJosch ( 2013-07-17 03:16:16 -0500 )edit

I'm putting in a fix to rviz now that checks if row_step * height == data.size() and gives an error if not. (instead of crashing)

hersh gravatar imagehersh ( 2013-07-17 05:26:53 -0500 )edit

Additionally, it looks like the row_step is not the problem, it is the width. Your python program says width = len( resultList2 ), which is the number of bytes in each row. It should be the number of points in each row, which would be len( resultList1 ).

hersh gravatar imagehersh ( 2013-07-17 05:57:16 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2013-06-19 03:29:12 -0500

Seen: 790 times

Last updated: Jul 17 '13