class CmdVelController():
"""
Converter for using FroboMind with stage.
Takes TwistStamped message from /fmSignals/cmd_vel and parses as Twist message on /cmd_vel
"""
def __init__(self):
# Get parameters
self.cmd_vel_sub = rospy.get_param("~cmd_vel_sub","/fmSignals/cmd_vel_sp")
self.cmd_vel_pub = rospy.get_param("~cmd_vel_sub","/fmSignals/cmd_vel")
self.frame_id = rospy.get_param("~frame_id",'base_link')
self.world_frame = rospy.get_param("~world_frame",'map')
self.rate = rospy.Rate(50.0)
# Init node
self.twist_sub = rospy.Subscriber(self.cmd_vel_sub, TwistStamped, self.onTwist )
self.twist_pub = rospy.Publisher(self.cmd_vel_pub, TwistStamped)
self.twist = Twist()
self.tf = TransformListener()
self.controller = Controller()
def onTwist(self,msg):
self.updateFeedback()
self.twist = self.controller.generateTwist(msg.twist.linear.x , msg.twist.angular.z)
self.twist_pub.publish(self.twist)
def updateFeedback(self):
try:
(position,heading) = self.__listen.lookupTransform( self.frame_id,self.world_frame,rospy.Time(0))
(roll,pitch,yaw) = tf.transformations.euler_from_quaternion(heading)
self.controller.setFeedback( Vector(position[0],position[1]), Vector(math.cos(yaw), math.sin(yaw)))
except (tf.LookupException, tf.ConnectivityException),err:
rospy.loginfo("could not locate vehicle")
def __init__(self):
# Get parameters
self.cmd_vel_sub = rospy.get_param("~cmd_vel_sub","/fmSignals/cmd_vel_sp")
self.cmd_vel_pub = rospy.get_param("~cmd_vel_sub","/fmSignals/cmd_vel")
self.frame_id = rospy.get_param("~frame_id",'base_link')
self.world_frame = rospy.get_param("~world_frame",'map')
self.rate = rospy.Rate(50.0)
# Init node
self.twist_sub = rospy.Subscriber(self.cmd_vel_sub, TwistStamped, self.onTwist )
self.twist_pub = rospy.Publisher(self.cmd_vel_pub, TwistStamped)
self.twist = Twist()
self.tf = TransformListener()
self.controller = Controller()