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

edit

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()