I am trying to generate a path of a polynomial shape for my robot to drive along. I need to transform the coordinates of the path from robot frame (body) to map frame (camerainit). For now, I am just trying to see if I can grab the transform from camerainit to body frame using tf.buffer.lookupTransform() and it's not working. ROS keeps saying "camera_init" doesn't exist, nor does "body" when I switch them.

I use "rosrun tf tfecho camerainit body" to check the if transforms exist and they do. It shows

At time 168.339

• Translation: [160.310, 12.501, -3.082]
• Rotation: in Quaternion [0.000, 0.097, -0.411, 0.907]

in RPY (radian) [-0.081, 0.176, -0.858]

in RPY (degree) [-4.614, 10.110, -49.174]

faketrajectorygenerator.cpp:

#include <vector>
#include <ros/ros.h>
#include <path_planner/fake_trajectory_generator.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/buffer.h>

TrajectoryGenerator::TrajectoryGenerator()
{
}

void TrajectoryGenerator::generateTrajectory(double x1,
                                         double x2,
                                         double x3,
                                         double y1,
                                         double y2,
                                         double y3,
                                         double stepsize)
{
double denominator = (x1 - x2) * (x1 - x3) * (x2 - x3);
double x1_squared = x1 * x1;
double x2_squared = x2 * x2;
double x3_squared = x3 * x3;
double a = (x3 * (y2 - y1) + x2 * (y1 - y3) + x1 * (y3 - y2)) / denominator;
double b = (x3_squared * (y1 - y2) + x2_squared * (y3 - y1) + x1_squared * (y2 - y3)) / denominator;
double c = 0;
double x = x1;

while (x <= x3)
{
    geometry_msgs::TransformStamped transform_to_robot = tf_buffer.lookupTransform("camera_init", "body", ros::Time(0));
    x += stepsize;
}
}

faketrajectorygenerator.h

#include <vector>
#include <ros/ros.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

struct Point
{
double x;
double y;
};

class TrajectoryGenerator
{

public:
TrajectoryGenerator();

void generateTrajectory(double x1,
                        double x2,
                        double x3,
                        double y1,
                        double y2,
                        double y3,
                        double stepsize);

std::vector<Point> getTrajectory()
{
    return trajectory;
}

private:
ros::NodeHandle     nh;
ros::Subscriber     sub_transforms;
double              x1;
double              x2;
double              x3;
double              y1;
double              y2;
double              y3;
double              stepsize;
tf2::Transform      T_cam_init_to_body;
std::vector<Point>  trajectory;
tf2_ros::Buffer     tf_buffer;
};

Any help is greatly appreciated