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

int main(int argc,char** argv){
    ros::init(argc,argv,"upbot_auto_recharge");
    ros::NodeHandle nh;
    ros::Publisher goal_pub = nh.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 10);

    geometry_msgs::PoseStamped goal;
    ros::Rate rate(1.0);
    /*
    while(ros:ok()){
        goal.header.frame_id = "map";
        goal.header.stamp = ros::Time::now();

        goal.pose.position.x = 0.0;
        goal.pose.position.y = 0.0;
        goal.pose.position.z = 0.0;

        goal.pose.orientation.x = 0.0;
        goal.pose.orientation.y = 0.0;
        goal.pose.orientation.z = 0.0;
        goal.pose.orientation.w = 0.0;
        
        goal_pub.publish(goal);
        rate.sleep();
    }
    */
    goal.header.frame_id = "map";
    goal.header.stamp = ros::Time::now();

    goal.pose.position.x = 0.0;
    goal.pose.position.y = 0.0;
    goal.pose.position.z = 0.0;

    goal.pose.orientation.x = 0.0;
    goal.pose.orientation.y = 0.0;
    goal.pose.orientation.z = 0.0;
    goal.pose.orientation.w = 0.0;
    
    goal_pub.publish(goal);

    ros::Duration(1.0).sleep(); // 等待发布器连接
    ROS_INFO("Goal published!");

    return 0;
}
