auto_dronesim.cpp

#include <ros/ros.h> 
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>

geometry_msgs::Twist twist;
float throttleX, throttleY, throttleZ;

int targX = 2;
int targY = 2;
int targZ = 2;
float speed = 0.5;

void pose_callback(const geometry_msgs::PoseStampedPtr &pose)
{
    // get current position from pose message
    float currX, currY, currZ;
    ROS_INFO("Callback triggered");
    currX = pose->pose.position.x;
    currY = pose->pose.position.y;
    currZ = pose->pose.position.z;
    ROS_INFO("x: %.2f, y: %.2f", pose->pose.position.x, pose->pose.position.y);

    // set z throttle based on direction to target
    if (currZ < targZ) {
        throttleZ = 1;
    }
    else {
        throttleZ = -1;
    }

    // set x throttle based on direction to target
    if (currX < targX) {
        throttleX = 1;
    }
    else {
        throttleX = -1;
    }

    // set y throttle based on direction to target
    if (currY < targY){
        throttleY = 1;
    }
    else {
        throttleY = -1;
    }

    // save throttle values into the global twist message
    twist.linear.x = currX*speed;
    twist.linear.y = currY*speed;
    twist.linear.z = currZ*speed;

}

int main(int argc, char **argv)
{
    ROS_INFO("Program Started");
    ros::init(argc, argv, "auto_dronesim");
    ros::NodeHandle node("~");
    ros::Publisher pub = node.advertise<geometry_msgs::Twist>("auto_dronesim/cmd_vel", 1);
    ros::NodeHandle n("~");
    ros::Subscriber pose_sub = n.subscribe("/slam_out_pose", 1, pose_callback);

    ros::Rate loop_rate(100);
    while (ros::ok()) {
        // publish the control message
        pub.publish(twist);
        
        // run callbacks
        ros::spinOnce();

        loop_rate.sleep();
    }
}