Ask Your Question
2

Rviz markers with rospy

asked 2016-04-12 20:49:38 -0600

jbourne gravatar image

updated 2016-04-13 10:06:51 -0600

I am trying to visually represent a "robot" as a sphere marker from the visualization_msgs, but I am unable to publish my self.robotMarker. When I rosrun this script I am able to see the message from the print statement, however when I rostopic echo /robotMarker, my node crashes. Am I initializing my Marker() correctly? Any help is appreciated.

#!/usr/bin/env python

import rospy
from std_msgs.msg import String
from geometry_msgs.msg import PointStamped
from visualization_msgs.msg import Marker

_ACTIONS = [('N', [0,-1]),('E', [-1,0]),('S',[0,1]),('W',[1,0]),('NE',[-1,-1]),('NW',[1,-1]),('SE',[-1,1]),('SW',[1,1]), ('STAY',[0,0])]
_X = 0
_Y = 1

class stateNode():

def __init__(self):
    pub = rospy.Publisher('state', PointStamped, queue_size=10)
    markerPub = rospy.Publisher('robotMarker', Marker, queue_size=10)
    rospy.Subscriber("action", String, self.move_callback)

    rospy.init_node('stateNode', anonymous=True)

    rate = rospy.Rate(1)
    self.state = PointStamped()

    # initial starting location I might want to move to the param list
    self.h = rospy.get_param("height", 100)
    self.w = rospy.get_param("width", 100)
    self.state.point.x = self.h
    self.state.point.y = self.w
    self.state.point.z = 0

    self.robotMarker = Marker()
    self.robotMarker.header.frame_id = "/Cmap"
    self.robotMarker.header.stamp    = rospy.get_rostime()
    self.robotMarker.ns = "robot"
    self.robotMarker.id = 0
    self.robotMarker.type = 2 # sphere
    self.robotMarker.action = 0
    self.robotMarker.pose.position = self.state.point
    self.robotMarker.pose.orientation.x = 0
    self.robotMarker.pose.orientation.y = 0
    self.robotMarker.pose.orientation.z = 0
    self.robotMarker.pose.orientation.w = 1.0
    self.robotMarker.scale.x = 1.0
    self.robotMarker.scale.y = 1.0
    self.robotMarker.scale.z = 1.0

    self.robotMarker.color.r = 0.0
    self.robotMarker.color.g = 1.0
    self.robotMarker.color.b = 0.0
    self.robotMarker.color.a = 1.0

    self.robotMarker.lifetime = 0

    while not rospy.is_shutdown():
        pub.publish(self.state)
        markerPub.publish(self.robotMarker)
        print "sending marker", self.robotMarker
        rate.sleep()

def move_callback(self, action):
    for i in _ACTIONS:
        if String(i[0]) == action:

            self.state.point.x = self.state.point.x + i[1][_X]
            self.state.point.y = self.state.point.y + i[1][_Y]

            if self.state.point.x>self.h:
                self.state.point.x = self.h
            if self.state.point.y>self.w:
                self.state.point.y = self.w

            self.state.point.z = 0
            self.robotMarker.pose.position = self.state.point
            # print i
            print self.state.point.x, self.state.point.y

stateNode()

This is what my terminal screen gives me:

Traceback (most recent call last):
  File "/home/jbourne/catkin_ws/src/plume_localization/scripts/stateNode.py", line 77, in <module>
    stateNode()
  File "/home/jbourne/catkin_ws/src/plume_localization/scripts/stateNode.py", line 56, in __init__
    markerPub.publish(self.robotMarker)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/topics.py", line 852, in publish
    self.impl.publish(data)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/topics.py", line 1036, in publish
    serialize_message(b, self.seq, message)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/msg.py", line 152, in ...
(more)
edit retag flag offensive close merge delete

Comments

Can you post the output from the "crash"?

William gravatar imageWilliam ( 2016-04-12 21:14:28 -0600 )edit

I am unable to upload the log file because of lack of points.

jbourne gravatar imagejbourne ( 2016-04-13 10:08:50 -0600 )edit

2 Answers

Sort by ยป oldest newest most voted
3

answered 2016-04-13 15:34:42 -0600

updated 2016-04-13 15:36:37 -0600

The issue is on the following line from your original code:

self.robotMarker.lifetime = 0

If you look at the definition of a visualization_msgs/Marker message, you'll see that the lifetime field is of type "duration" which is a built-in type that needs to be a rospy.Duration instance in Python (see here). In the line of code above, you are setting the value to an integer which is incorrect. That is what causes the

AttributeError: 'int' object has no attribute 'secs'

line in your output log.

Edit

You could change the line to something like

self.robotMarker.lifetime = rospy.Duration(0)
edit flag offensive delete link more

Comments

Thank you for the clarification. Yes this seems to fix my bug.

jbourne gravatar imagejbourne ( 2016-04-13 19:05:25 -0600 )edit
-1

answered 2016-04-13 12:45:11 -0600

jbourne gravatar image

I placed my Marker within my while loop and this fixed my problem.

#!/usr/bin/env python

import rospy
from std_msgs.msg import String
from geometry_msgs.msg import PointStamped
from visualization_msgs.msg import Marker

_ACTIONS = [('N', [0,-1]),('E', [-1,0]),('S',[0,1]),('W',[1,0]),('NE',[-1,-1]),('NW',[1,-1]),('SE',[-1,1]),('SW',[1,1]), ('STAY',[0,0])]
_X = 0
_Y = 1

class stateNode():

def __init__(self):
    pub = rospy.Publisher('state', PointStamped, queue_size=10)
    markerPub = rospy.Publisher('robotMarker', Marker, queue_size=1)
    rospy.Subscriber("action", String, self.move_callback)

    rospy.init_node('stateNode', anonymous=True)

    rate = rospy.Rate(1)
    self.state = PointStamped()

    # initial starting location I might want to move to the param list
    self.h = rospy.get_param("height", 100)
    self.w = rospy.get_param("width", 100)
    self.state.point.x = self.h
    self.state.point.y = self.w
    self.state.point.z = 0



    while not rospy.is_shutdown():
        pub.publish(self.state)
        # markerPub.publish(self.robotMarker)

        self.robotMarker = Marker()
        self.robotMarker.header.frame_id = "Cmap"
        self.robotMarker.header.stamp    = rospy.Time.now()
        self.robotMarker.ns = "robot"
        self.robotMarker.id = 0
        self.robotMarker.type = Marker.SPHERE
        self.robotMarker.action = Marker.ADD
        self.robotMarker.pose.position = self.state.point
        self.robotMarker.pose.position.z = 1 # shift sphere up

        self.robotMarker.pose.orientation.x = 0
        self.robotMarker.pose.orientation.y = 0
        self.robotMarker.pose.orientation.z = 0
        self.robotMarker.pose.orientation.w = 1.0
        self.robotMarker.scale.x = 1.0
        self.robotMarker.scale.y = 1.0
        self.robotMarker.scale.z = 1.0

        self.robotMarker.color.r = 1.0
        self.robotMarker.color.g = 0.0
        self.robotMarker.color.b = 0.0
        self.robotMarker.color.a = 1.0

        markerPub.publish(self.robotMarker)
        print "sending marker", self.robotMarker.pose.position
        rate.sleep()

def move_callback(self, action):
    for i in _ACTIONS:
        if String(i[0]) == action:

            self.state.point.x = self.state.point.x + i[1][_X]
            self.state.point.y = self.state.point.y + i[1][_Y]

            if self.state.point.x>self.h:
                self.state.point.x = self.h
            if self.state.point.y>self.w:
                self.state.point.y = self.w

            self.state.point.z = 0
            # self.robotMarker.pose.position = self.state.point
            # self.ellipse = self.state.point

            # print i
            print self.state.point.x, self.state.point.y

stateNode()

edit flag offensive delete link more

Comments

2

This answer that you provided is incorrect. The while loop that you added is not what fixed your issue. Please see my answer for an explanation of what is actually wrong.

jarvisschultz gravatar imagejarvisschultz ( 2016-04-13 15:31:04 -0600 )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

1 follower

Stats

Asked: 2016-04-12 20:49:38 -0600

Seen: 4,638 times

Last updated: Apr 13 '16