20210609循环有问题

# include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "pub_forpath_test/planning_input.h"

class SubscribeAndPublish_startandgoal
{
public:
  SubscribeAndPublish_startandgoal()
  {
    pub_start= n_.advertise<pub_forpath_test::planning_input>("/position_world", 50);
    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        for (int i = 0; i < 20; i++)
        {
            pub_forpath_test::planning_input map_start;
            map_start.header.frame_id = "map";
            map_start.header.seq = 0;
            map_start.header.stamp = ros::Time();
            map_start.pose.x = 0.00+i;
            map_start.pose.y = 7.00;
            map_start.pose.theta = 0;
            pub_start.publish(map_start);
            ROS_INFO("we publish the position_world");
            loop_rate.sleep();
        }


    }
    
    
    }

private:
  ros::NodeHandle n_; 
 ros::Publisher pub_start;
//   ros::Subscriber sub_;
};



int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"pub_topic_test");
    SubscribeAndPublish_startandgoal SAPObject1;
    ros::spin();
    return 0;
}

# include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "pub_forpath_test/planning_input.h"

class SubscribeAndPublish_startandgoal
{
public:
  SubscribeAndPublish_startandgoal()
  {
    pub_start= n_.advertise<pub_forpath_test::planning_input>("/position_world", 50);
    ros::Rate loop_rate(1);
    // while (ros::ok())
    // {
        for (int i = 0; i < 20; i++)
        {
            pub_forpath_test::planning_input map_start;
            map_start.header.frame_id = "map";
            map_start.header.seq = 0;
            map_start.header.stamp = ros::Time();
            map_start.pose.x = 0.00+i;
            map_start.pose.y = 7.00;
            map_start.pose.theta = 0;
            pub_start.publish(map_start);
            ROS_INFO("we publish the position_world");
            loop_rate.sleep();
        }


    // }
    
    
    }

private:
  ros::NodeHandle n_; 
 ros::Publisher pub_start;
//   ros::Subscriber sub_;
};



int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"pub_topic_test");
    SubscribeAndPublish_startandgoal SAPObject1;
    ros::spin();
    return 0;
}

测试程序

# include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "pub_forpath_test/planning_input.h"
# include "tf/tf.h"
#include <iostream>

class SubscribeAndPublish_startandgoal
{
public:
  SubscribeAndPublish_startandgoal()
  {
    pub_start= n_.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1);
    pub_goal= n_.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1);
    sub_gps = n_.subscribe("/position_world", 50,&SubscribeAndPublish_startandgoal::do_positionworld,this);
   
    }

    // 输入当前位置作为起点，终点位置暂时定为不变
    void do_positionworld(const pub_forpath_test::planning_input& gps_info){

      ros::Rate loop_rate(1);
      while (ros::ok())
      {      
        geometry_msgs::PoseWithCovarianceStamped map_start;
      map_start.header.frame_id="map";
      map_start.header.seq=0;
      map_start.header.stamp=ros::Time();
      map_start.pose.pose.position.x=double(gps_info.pose.x);
      map_start.pose.pose.position.y=double(gps_info.pose.y);
      map_start.pose.pose.position.z=0.00;

      map_start.pose.pose.orientation=tf::createQuaternionMsgFromRollPitchYaw(0,0,gps_info.pose.theta);
      pub_start.publish(map_start) ;

      ROS_INFO("we publish the current position");
      
        geometry_msgs::PoseStamped map_goal;
        map_goal.header.frame_id = "map";
        map_goal.header.seq = 0;
        map_goal.header.stamp = ros::Time();
        map_goal.pose.position.x = 60.00;
        map_goal.pose.position.y = 7.00;
        map_goal.pose.position.z = 0.00;
        map_goal.pose.orientation.x = 0;
        map_goal.pose.orientation.y = 0;
        map_goal.pose.orientation.z = 0;
        map_goal.pose.orientation.w = 1;

        pub_goal.publish(map_goal);
        ROS_INFO("we publish the robot's position and orientaion");
        loop_rate.sleep();
      }
    }

private:
 ros::NodeHandle n_; 
 ros::Publisher pub_start;
 ros::Publisher pub_goal;
 ros::Subscriber sub_gps;
};



int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"pub_startandgoal_HA");
    SubscribeAndPublish_startandgoal SAPObject1;
    ros::spin();
    return 0;
}

rostopic echo /initialpose