Set transform reference from camera frame to map frame

edit

Hello all.

I am trying to develop a Follow me application. I am getting the position of a person through a transform referenced to the camera which detect and locate the person. In a script I am setting a goal dynamically to this position. The script is the following:

#!/usr/bin/env python3

import rospy
import math
from math import sin, cos, pi
import tf
import actionlib

#Messages
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from tf.msg import tfMessage
from geometry_msgs.msg import TransformStamped, Transform, Vector3, Quaternion

class FOLLOW_ME_CLASS:

# Function: __init__
#
# Comments
# ----------
#
# Parameters
# ----------
#
# Returns
# -------
    def __init__(self):
        self.x=0
        self.y=0
        self.x_odom=0
        self.y_odom=0
        self.x_base_link=0
        self.y_base_link=0
        self.qx=0
        self.qy=0
        self.qz=0
        self.qw=0

# Function: main
#
# Comments
# ----------
#
# Parameters
# ----------
#
# Returns
# -------

    def main(self):

        rospy.init_node('follow_me')
        rospy.loginfo("Iniciado nodo follow_me")

        #self.tf_listener=tf.TransformListener()

        #Publishers
        #self.data_pub = rospy.Publisher("wheel_state", JointState, queue_size=100)

        #Subscriber
        rospy.Subscriber("/tf", tfMessage, self.callback_tf)

        T=rospy.Rate(1)
        while not rospy.is_shutdown():
            self.dynamic_goal()
            T.sleep()

    def callback_tf(self, msg):
        if msg.transforms[0].child_frame_id == "odom":
            self.x_odom=msg.transforms[0].transform.translation.x
            self.y_odom=msg.transforms[0].transform.translation.y
        if msg.transforms[0].child_frame_id == "base_link":
            self.x_base_link=msg.transforms[0].transform.translation.x
            self.y_base_link=msg.transforms[0].transform.translation.y
        if msg.transforms[0].child_frame_id == "person":
            x_person=msg.transforms[0].transform.translation.x
            y_person=msg.transforms[0].transform.translation.y

            self.qx=msg.transforms[0].transform.rotation.x
            self.qy=msg.transforms[0].transform.rotation.y
            self.qz=msg.transforms[0].transform.rotation.z
            self.qw=msg.transforms[0].transform.rotation.w

            #self.x=self.x_odom+self.x_base_link+x_person
            #self.y=self.y_odom+self.y_base_link+y_person
            self.x=x_person
            self.y=y_person

# Function: dynamic_goal
#
# Comments
# ----------
#
# Parameters
# ----------
#
# Returns
# -------
    def dynamic_goal(self):
        #(trans,rot) = self.tf_listener.lookupTransform('/person_filt', '/map', rospy.Time(0))
        #print(trans)
        #print(rot)
        # Create an action client called "move_base" with action definition file "MoveBaseAction"
        client = actionlib.SimpleActionClient('move_base',MoveBaseAction)

        # Waits until the action server has started up and started listening for goals.
        client.wait_for_server()

        # Creates a new goal with the MoveBaseGoal constructor
        goal = MoveBaseGoal()
        goal.target_pose.header.frame_id = "camera_depth_frame"
        goal.target_pose.header.stamp = rospy.Time.now()
        goal.target_pose.pose.position.x = self.x
        goal.target_pose.pose.position.y = self.y
        #quat = tf.transformations.quaternion_from_euler(0, 0, 0)
        goal.target_pose.pose.orientation.x = self.qx
        goal.target_pose.pose.orientation.y = self.qy
        goal.target_pose.pose.orientation.z = self.qz
        goal.target_pose.pose.orientation.w = self.qw
        # Sends the goal to the action server.
        client.send_goal(goal)
        rospy.loginfo("Goal updated")
        #wait = client.wait_for_result()

if __name__ == '__main__':

    follow_me=FOLLOW_ME_CLASS()
    follow_me.main()

As you can see, I put the header ID of the goal as the camera frame, because this is the reference of the position that I get, but I got the following error:

[ERROR] [1639138227.154849811]: The goal pose passed to this planner must be in the map frame.  It is instead in the camera_depth_frame frame.

I ... (more)