Ask Your Question
0

How to get interactive marker's pose published all the time

asked 2017-08-10 22:01:57 -0600

Megacephalo gravatar image

updated 2017-08-14 20:41:25 -0600

Hi all,

I am trying to write a flexible marker to represent a fake charging station to which the robot must approach and dock. For that, I am using interactive marker for the fake device, and I expect it to publish the pose no matter if I clicked, drag, release or not clicking the marker at all. However, what I can do, is to get its pose only via feedback with its own callback function (insert() ). My question is, How can I get my interactive marker to publish its pose not depending on any clicking? Thanks in advance!

My piece of code looks like this:

import math
import copy
import rospy
import tf

from geometry_msgs.msg import Quaternion, PoseStamped, Pose, Point
from interactive_markers.interactive_marker_server import *
from interactive_markers.menu_handler import *
from visualization_msgs.msg import *
from laser_tool.msg import *


class single_station_broadcaster:
    def __init__(self):
        self.server = InteractiveMarkerServer("AutoChargingStation")

        self.pose_pub = rospy.Publisher('charging_station_pose', PoseStamped, queue_size=500)

        self.paramCfg()



def paramCfg(self):
    self.init_x_ = rospy.get_param('~init_x', 0)
    self.init_y_ = rospy.get_param('~init_y', 0)
    self.init_a_ = rospy.get_param("~init_a", 0)
    self.init_a_rad = math.radians(self . init_a_)
    self.global_frame = rospy.get_param('~global_frame', 'map')

def makeBoxControl(self, msg):
    control = InteractiveMarkerControl()
    control.always_visible = True
    control.markers.append(self.makeBox(msg))
    msg.controls.append(control)
    return control

def makeBox(self, msg):
    marker = Marker()

    marker.type = Marker.CUBE
    marker.scale.x = msg.scale * 0.45
    marker.scale.y = msg.scale * 0.45
    marker.scale.z = msg.scale * 0.45
    marker.color.r = 0.0
    marker.color.g = 0.5
    marker.color.b = 0.5
    marker.color.a = 0.5
    return marker

def createMarker(self):
    self.int_marker = InteractiveMarker()
    int_marker = self.int_marker
    int_marker.header.frame_id = self.global_frame
    int_marker.header.stamp = rospy.Time.now()
    int_marker.pose.position.x = self.init_x_
    int_marker.pose.position.y = self.init_y_
    int_marker.pose.position.z = 0.2

    quaternion = tf.transformations.quaternion_from_euler(0, 0, self . init_a_rad)
    int_marker.pose.orientation.x = quaternion[0]
    int_marker.pose.orientation.y = quaternion[1]
    int_marker.pose.orientation.z = quaternion[2]
    int_marker.pose.orientation.w = quaternion[3]

    int_marker.scale = 1

    int_marker.name = "AutoChargingStation"
    int_marker.description = "Charging station"


    self . makeBoxControl(int_marker)
    control = InteractiveMarkerControl()

    # Custom move on plane
    control.orientation.w = 1
    control.orientation.x = 0
    control.orientation.y = 1
    control.orientation.z = 0
    control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
    int_marker.controls.append(copy.deepcopy(control))

    # make a box which also moves on the plane
    control.markers.append(self . makeBox(int_marker))
    control.always_visible = True
    int_marker.controls.append(copy.deepcopy(control))

    self.server.insert(int_marker, self . publish_pose_callback)

def applyCahnges(self):
    self.server.applyChanges()

def publish_pose_callback(self, feedback):
    # Publish the pose of the charging station
    self.pose = PoseStamped()
    self.pose.header = Header()
    self.pose.header.frame_id = self.global_frame
    self.pose.header.stamp = rospy.Time.now()
    self.pose.pose = Pose()
    self.pose.pose.position = Point()
    self.pose.pose.orientation = Quaternion()

    self.pose.pose.position = feedback.pose.position
    self.pose.pose.orientation = feedback.pose.orientation

    self.pose_pub.publish(self.dummyPose)

 if __name__ == '__main__':
     rospy.init_node('single_charging_station_broadcaster', anonymous=True)

     try:
         chStation = single_station_broadcaster()
         chStation.createMarker()
         chStation.applyCahnges()
     except rospy.ROSInterruptException:
         pass

     rospy . spin()
edit retag flag offensive close merge delete

Comments

1

There are missing indents and extra spaces around dots that make the python script hard to read.

lucasw gravatar imagelucasw ( 2017-08-10 23:46:08 -0600 )edit
1

@locasw Thank you for your advice. I eliminated the spaces around the dots. Not sure about the indent. Hopefully the code may be more readable than before.

Megacephalo gravatar imageMegacephalo ( 2017-08-13 20:26:09 -0600 )edit
1

The indentation issue is with many of the method definitions and the try...except block at the end.

jayess gravatar imagejayess ( 2017-08-13 21:07:20 -0600 )edit

@jayess Thank you for point this out. It is fixed! Hurray !

Megacephalo gravatar imageMegacephalo ( 2017-08-14 20:40:23 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2017-08-14 09:42:18 -0600

lucasw gravatar image

Take a look at http://wiki.ros.org/rospy/Overview/Ti...

You'll want to create a timer callback that publishes self.pose, it can update at any rate you want.

edit flag offensive delete link more

Comments

@lucasw Thank you !! I will try tomorrow. If there is any progress, I will post it here !!

Megacephalo gravatar imageMegacephalo ( 2017-08-15 04:31:28 -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

2 followers

Stats

Asked: 2017-08-10 22:01:57 -0600

Seen: 618 times

Last updated: Aug 14 '17