How to get interactive marker's pose published all the time
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()
There are missing indents and extra spaces around dots that make the python script hard to read.
@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.
The indentation issue is with many of the method definitions and the
try...except
block at the end.@jayess Thank you for point this out. It is fixed! Hurray !