22 from geometry_msgs.msg
import PoseStamped
23 from geometry_msgs.msg
import Pose
24 from geometry_msgs.msg
import Twist
27 from tf.transformations
import euler_from_quaternion
32 NaoqiNode.__init__(self,
'naoqi_moveto_listener')
36 self.
goal_sub = rospy.Subscriber(
'move_base_simple/goal', PoseStamped, self.
goalCB)
42 rospy.loginfo(
"Connecting to NaoQi at %s:%d", self.
pip, self.
pport)
50 poseStamped.header.stamp = rospy.Time(0)
52 robotToTarget1 = self.listener.transformPose(
"/base_footprint", poseStamped)
53 except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException)
as e:
54 rospy.logerr(
"Error while transforming pose: %s", str(e))
56 quat = robotToTarget1.pose.orientation
57 (roll,pitch,yaw) = euler_from_quaternion((quat.x, quat.y, quat.z, quat.w))
58 self.motionProxy.moveTo(robotToTarget1.pose.position.x, robotToTarget1.pose.position.y, yaw)
61 rospy.logdebug(
"cmd_vel: %f %f %f", twist.linear.x, twist.linear.y, twist.angular.z)
63 if abs(twist.linear.x)<eps
and abs(twist.linear.y)<eps
and abs(twist.angular.z)<eps:
64 self.motionProxy.moveToward(0,0,0)
66 self.motionProxy.moveToward(twist.linear.x, twist.linear.y, twist.angular.z)
def goalCB(self, poseStamped)
def get_proxy(self, name, warn=True)