simple.c

来自「机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。」· C语言 代码 · 共 67 行

// simple.c - works with simple.world
// compile with:
//    gcc -Wall -g3 `pkg-config --cflags gazebo` -c -o simple.o simple.c
//    gcc simple.o -o simple `pkg-config --libs gazebo` 
// Thanks to Kevin Knoedler for various fixes.

#include <stdio.h>
#include <gazebo.h>

int main (int argc, char *argv[])
{
  int i;
  gz_client_t *client;
  gz_position_t *position;
  int server_id;
  int client_id;

  // Assume one server and one client
  server_id = 0;
  client_id = 0;

  // Print all errors
  gz_error_init(1, 9);

  // Create a client object
  client = gz_client_alloc();  

  // Connect to the server
  if (gz_client_connect_wait(client, server_id, client_id) != 0)
    return -1;

  // Create a position interface
  position = gz_position_alloc();

  // Connect to device on server
  if (gz_position_open(position, client, "robot1") != 0)
    return -1;

  // Read some data
  for (i = 0; i < 10; i++)
  {
    // Wait for new data
    gz_client_wait(client);
    
    // Lock it
    gz_position_lock(position, 1);

    // Print the current odometric position
    printf("%0.3f %0.3f\n", 
           position->data->pos[0], position->data->pos[1]); 

    // Unlock it
    gz_position_unlock(position);
  }

  // Close the interface and free object
  gz_position_close(position);
  gz_position_free(position);

  // Close the connection and free the client object
  gz_client_disconnect(client);
  gz_client_free(client);

  return 0;
}