Код C, который необходимо перевести на C ++ ROS - PullRequest
Новая
       88

Код C, который необходимо перевести на C ++ ROS

0 голосов
/ 05 мая 2020

Я хочу перевести код C в код ros на C ++. Это то, что у меня ниже, - это код C ++ ROS. Я хочу, чтобы камера распознавала объекты на веботах с помощью ros. Поскольку у веботов есть образцы документации, сделанные на C, мне нужна помощь в переводе этого кода на ROS cpp 

  void CameraCallback(const webots_ros::RecognitionObject::ConstPtr &image) {
     int i;
     int j;
     int number_of_objects;
     const *objects;
    // int number_of_objects = getRecognitionNumberOfObjects() const;(camera);
      //ROS_INFO("\nRecognized %d objects.\n", number_of_objects);
      ROS_INFO("\nRecognized %d objects.\n", number_of_objects);
    /* Get and display all the objects information */
    const RecognitionObject *objects = wb_camera_recognition_get_objects(camera);
    for (i = 0; i < number_of_objects; ++i) {
      ROS_INFO("Model of object %d: %s\n", i, objects[i].model);
      ROS_INFO("Id of object %d: %d\n", i, objects[i].id);
      ROS_INFO("Relative position of object %d: %lf %lf %lf\n", i, objects[i].position[0], objects[i].position[1],
             objects[i].position[2]);
      ROS_INFO("Relative orientation of object %d: %lf %lf %lf %lf\n", i, objects[i].orientation[0], objects[i].orientation[1],
             objects[i].orientation[2], objects[i].orientation[3]);
      ROS_INFO("Size of object %d: %lf %lf\n", i, objects[i].size[0], objects[i].size[1]);
      ROS_INFO("Position of the object %d on the camera image: %d %d\n", i, objects[i].position_on_image[0],
             objects[i].position_on_image[1]);
      ROS_INFO("Size of the object %d on the camera image: %d %d\n", i, objects[i].size_on_image[0], objects[i].size_on_image[1]);
      for (j = 0; j < objects[i].number_of_colors; ++j)
      ROS_INFO("- Color %d/%d: %lf %lf %lf\n", j + 1, objects[i].number_of_colors, objects[i].colors[3 * j],
               objects[i].colors[3 * j + 1], objects[i].colors[3 * j + 2]);
    }

Вот исходный код C. Я пытаюсь интегрировать это в webots. 

#include <stdio.h>
#include <webots/camera.h>
#include <webots/camera_recognition_object.h>
#include <webots/motor.h>
#include <webots/robot.h>

#define SPEED 1.5
#define TIME_STEP 64

int main() {
  WbDeviceTag camera, left_motor, right_motor;
  int i, j;

  wb_robot_init();

  /* Get the camera device, enable it and the recognition */
  camera = wb_robot_get_device("camera");
  wb_camera_enable(camera, TIME_STEP);
  wb_camera_recognition_enable(camera, TIME_STEP);

  /* get a handler to the motors and set target position to infinity (speed control). */
  left_motor = wb_robot_get_device("left wheel motor");
  right_motor = wb_robot_get_device("right wheel motor");
  wb_motor_set_position(left_motor, INFINITY);
  wb_motor_set_position(right_motor, INFINITY);
  wb_motor_set_velocity(left_motor, 0.0);
  wb_motor_set_velocity(right_motor, 0.0);

  /* Set the motors speed */
  wb_motor_set_velocity(left_motor, -SPEED);
  wb_motor_set_velocity(right_motor, SPEED);

  /* Main loop */
  while (wb_robot_step(TIME_STEP) != -1) {
    /* Get current number of object recognized */
    int number_of_objects = wb_camera_recognition_get_number_of_objects(camera);
    printf("\nRecognized %d objects.\n", number_of_objects);

    /* Get and display all the objects information */
    const WbCameraRecognitionObject *objects = wb_camera_recognition_get_objects(camera);
    for (i = 0; i < number_of_objects; ++i) {
      printf("Model of object %d: %s\n", i, objects[i].model);
      printf("Id of object %d: %d\n", i, objects[i].id);
      printf("Relative position of object %d: %lf %lf %lf\n", i, objects[i].position[0], objects[i].position[1],
             objects[i].position[2]);
      printf("Relative orientation of object %d: %lf %lf %lf %lf\n", i, objects[i].orientation[0], objects[i].orientation[1],
             objects[i].orientation[2], objects[i].orientation[3]);
      printf("Size of object %d: %lf %lf\n", i, objects[i].size[0], objects[i].size[1]);
      printf("Position of the object %d on the camera image: %d %d\n", i, objects[i].position_on_image[0],
             objects[i].position_on_image[1]);
      printf("Size of the object %d on the camera image: %d %d\n", i, objects[i].size_on_image[0], objects[i].size_on_image[1]);
      for (j = 0; j < objects[i].number_of_colors; ++j)
        printf("- Color %d/%d: %lf %lf %lf\n", j + 1, objects[i].number_of_colors, objects[i].colors[3 * j],
               objects[i].colors[3 * j + 1], objects[i].colors[3 * j + 2]);
    }
  }

  wb_robot_cleanup();

Мне нужна только часть объекта распознавания камеры ... Поскольку на данный момент я не понимаю, как сделать понятным для ros c ++

1 Ответ

1 голос
/ 06 мая 2020

Основная проблема в вашем коде заключается в том, что вы смешиваете ROS service / topi c API с API Webots, в своем узле вы не должны использовать API Webots вообще.

Тогда ваш * Функция обратного вызова 1003 * получит непосредственно объект webots_ros::RecognitionObject для каждого объекта, распознаваемого камерой. Следовательно, нет необходимости создавать al oop, этот обратный вызов уже будет вызываться один раз для каждого объекта.

Здесь вы найдете определение сообщения webots_ros::RecognitionObject: http://docs.ros.org/melodic/api/webots_ros/html/msg/RecognitionObject.html

В своей функции обратного вызова вы можете напрямую получить атрибуты, например: 

void CameraCallback(const webots_ros::RecognitionObject::ConstPtr &image) {
  double x = image->position.x
  // ...
}
...