在gazebo / c ++中获取信息的最佳方法

Posted 2023-02-22

【中文标题】在gazebo / c ++中获取信息的最佳方法

【英文标题】：best way to get information out of class in gazebo/c++

【发布时间】：2020-02-20 20:32:50

【问题描述】：

我正在尝试从某个 gazebo c++ 代码启动的类中获取信息。使这更加令人困惑的是类的调用方式（gazebo::transport::SubscriberPtr sub = node->Subscribe("~/pose/info", posesStampedCallback);）。我试图在更新时从该类中获取值，例如 x_pos。为了规避这种混乱，我尝试使用全局变量（x_pos）来提取数据，但似乎没有受到影响。关于如何提取变量的任何想法？我想我不能只将“voidposesStampedCallback”更改为“doubleposesStampedCallback”，虽然我不知道为什么......

#include <gazebo/gazebo_config.h>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <stdio.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <string.h>
#define PORT 8090 
#include <pthread.h>

#include <string>

// Gazebo's API has changed between major releases. These changes are
// accounted for with #if..#endif blocks in this file.
#if GAZEBO_MAJOR_VERSION < 6
#include <gazebo/gazebo.hh>
#else
#include <gazebo/gazebo_client.hh>
#endif

double x_pos = 0;

void posesStampedCallback(ConstPosesStampedPtr &posesStamped)

  std::cout << posesStamped->DebugString();

  ::google::protobuf::int32 sec = posesStamped->time().sec();
  ::google::protobuf::int32 nsec = posesStamped->time().nsec();
  std::cout << "Read time: sec: " << sec << " nsec: " << nsec << std::endl;
  for (int i =0; i < posesStamped->pose_size(); ++i)
  
    const ::gazebo::msgs::Pose &pose = posesStamped->pose(i);
    std::string name = pose.name();
//    if (name == std::string("my_velodyne")) 
      const ::gazebo::msgs::Vector3d &position = pose.position();

      double x = position.x();
      double y = position.y();
      double z = position.z();
    x_pos = 5;
      std::cout << "Read position: x: " << x
          << " y: " << y << " z: " << z << std::endl;
//    
  


/////////////////////////////////////////////////
int main(int _argc, char **_argv)


        printf("started connecting to python\n");    
        int sock = 0, valread;
        struct sockaddr_in serv_addr;
        if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0)
        
                printf("\n Socket creation error \n");
        
        serv_addr.sin_family = AF_INET;
        serv_addr.sin_port = htons(PORT);
        // Convert IPv4 and IPv6 addresses from text to binary form 
        if(inet_pton(AF_INET, "127.0.0.1", &serv_addr.sin_addr)<=0)
        
                printf("\nInvalid address/ Address not supported \n");
        
        if (connect(sock, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0)
        
                printf("\nConnection Failed \n");
        

          // Create our node for communication
    gazebo::transport::NodePtr node(new gazebo::transport::Node());
    node->Init();

    // Listen to Gazebo pose information topic
         gazebo::transport::SubscriberPtr sub = node->Subscribe("~/pose/info", posesStampedCallback);


    while(1)
//      const ::gazebo::msgs::Pose &pose = posesStampedCallback->pose(1);
//      const ::gazebo::msgs::Vector3d &position = pose.position();
//      double x = position.x();
        std::string hello_string = std::to_string(x_pos);
        char hello[hello_string.size() + 1];
        strcpy(hello, hello_string.c_str());
//      char *hello = "request";
        char buffer[1024] = 0;
        send(sock , hello , strlen(hello) , 0 );
        printf("request sent\n");
    printf(hello);
        valread = read( sock , buffer, 1024);
        printf("\n%s\n",buffer );
        int speed = atof(buffer);
        printf("finished connecting to python\n");




    // Load gazebo as a client
#if GAZEBO_MAJOR_VERSION < 6
  gazebo::setupClient(_argc, _argv);
#else
  gazebo::client::setup(_argc, _argv);
#endif

  // Create our node for communication
  gazebo::transport::NodePtr node(new gazebo::transport::Node());
  node->Init();

  // Publish to the  velodyne topic
  gazebo::transport::PublisherPtr pub =
    node->Advertise<gazebo::msgs::Vector3d>("~/my_velodyne/vel_cmd");

  // Wait for a subscriber to connect to this publisher
  pub->WaitForConnection();

  // Create a a vector3 message
  gazebo::msgs::Vector3d msg;

  // Set the velocity in the x-component
#if GAZEBO_MAJOR_VERSION < 6
  gazebo::msgs::Set(&msg, gazebo::math::Vector3(speed, 0, 0));
#else
  gazebo::msgs::Set(&msg, ignition::math::Vector3d(speed, 0, 0));
#endif

  // Send the message
  pub->Publish(msg);

  // Make sure to shut everything down.
#if GAZEBO_MAJOR_VERSION < 6
  gazebo::shutdown();
#else
  gazebo::client::shutdown();
#endif

【问题讨论】：

你能把你的代码减少到一个最小的可重现的例子吗***.com/help/minimal-reproducible-example

我可以减少代码，但我不确定如果没有凉亭堆栈的其余部分，它的重现性如何。这个想法是在更大的凉亭世界运行时运行此脚本。此代码插入并发送值（该部分工作正常）。当我试图从凉亭中提取值时，我遇到了问题。

至少它会使其更具可读性。它还将帮助您确定程序的哪些部分实际上导致了问题，并且在将问题减少到尽可能简单的内核时，您可能会自己找到解决方案。

【参考方案1】：

我想通了（至少我正在激活那个回调函数）（虽然它给了我一个段错误，但这是另一个问题）

无论如何，问题是我在尝试回调之前没有初始化客户端。我需要 ''' 凉亭::client::setup(_argc, _argv); ''' 回调之前