как создать условие, используя данные ros odom posose?

Question

У меня есть мобильный автономный робот, который перемещается с помощью Рос. Моя цель - связаться с rosserial через arduino и подписаться на тему odom. Я хотел бы поставить некоторые условия в отношении данных о воздействии, которые я получил после того, как стал членом Odom. Для того, чтобы иметь возможность выполнять некоторые операции в определенных точках с координатами на моей карте. на простом примере

      if (pose_x>2.17 &&pose_x<4.00 &&pose_y<7.6 &&pose_y>9.5 ){
digitalWrite(led_pin, HIGH - digitalRead(13));
}

Я хочу добавить такие условия, но не знаю, как это сделать. Я не очень разбираюсь в программном обеспечении. Ниже показан код, который я пытаюсь сделать с помощью Arduino. Возможно, я был немного глуп. Мне все еще очень не хватает. Буду благодарен всем за вашу помощь.

      #include <ros.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>


ros::NodeHandle nh;
ros::Time current_time;

float TIME;
float pose_x, pose_y, angle_z;
geometry_msgs::Quaternion odom_quat;
tf::TransformBroadcaster odom_broadcaster;

std_msgs::String message;
ros::Publisher server_pub("/server_messages", &message);

const int button1_pin = 6;
const int button2_pin = 7;
const int led_pin = 13;

char button1[13] = "2";
char button2[13] = "3";


bool last_reading1;
long last_debounce_time1 = 0;
bool last_reading2;
long last_debounce_time2 = 0;
long debounce_delay = 50;
bool published1 = true;
bool published2 = true;


void odomCallback(const nav_msgs::Odometry & msg) {
  nav_msgs::Odometry odom;
  odom.header.stamp = current_time;
  geometry_msgs::TransformStamped odom_trans;
  odom_trans.header.stamp = current_time;


  odom_trans.transform.translation.x = pose_x;
  odom_trans.transform.translation.y = pose_y;
  odom_trans.transform.translation.z = 0.0;
  odom_trans.transform.rotation = odom_quat;

  odom.pose.pose.position.x = pose_x;
  odom.pose.pose.position.y = pose_y;
  odom.pose.pose.position.z = 0.0;
  odom.pose.pose.orientation = odom_quat;


  if (pose_x>-8 && pose_x<10 && pose_y>-8 && pose_y<8) {

    digitalWrite(led_pin, HIGH - digitalRead(13));
  }
  else {
    digitalWrite(led_pin, LOW - digitalRead(13));

  }


}

ros::Subscriber <nav_msgs::Odometry> sub("/odom", odomCallback);

void setup()
{
  nh.initNode();
  nh.advertise(server_pub);
  nh.subscribe(sub);

  //initialize an LED output pin
  //and a input pin for our push button
  pinMode(led_pin, OUTPUT);
  pinMode(button1_pin, INPUT);
  pinMode(button2_pin, INPUT);

  //Enable the pullup resistor on the button
  digitalWrite(button1_pin, HIGH);
  digitalWrite(button2_pin, HIGH);

  //The button is a normally button
  last_reading1 = ! digitalRead(button1_pin);
  last_reading2 = ! digitalRead(button2_pin);

}

void loop()
{

  bool reading1 = ! digitalRead(button1_pin);
  bool reading2 = ! digitalRead(button2_pin);

  if (last_reading1 != reading1) {
    last_debounce_time1 = millis();
    published1 = false;
  }


  if (last_reading2 != reading2) {
    last_debounce_time2 = millis();
    published2 = false;
  }

  //if the button value has not changed for the debounce delay, we know its stable
  if ( !published1 == 1 && (millis() - last_debounce_time1)  > debounce_delay) {
    digitalWrite(led_pin, reading1);
    message.data = button1 ;
    server_pub.publish(&message);
    published1 = true;
  }

  if ( !published2 == 1 && (millis() - last_debounce_time2)  > debounce_delay) {
    digitalWrite(led_pin, reading2);
    message.data = button2 ;
    server_pub.publish(&message);
    published2 = true;
  }

  last_reading1 = reading1;
  last_reading2 = reading2;

  nh.spinOnce();
}

Answer 1

Похоже, вы вообще не используете данные из odomCallback.

Похоже, значения pose_x и pose_y никогда не устанавливаются. Попробуйте добавить это в начало функции odomCallback (не проверено):

      // set pose_x and pose_y to their most recent values
pose_x = msg.pose.pose.position.x;
pose_y = msg.pose.pose.position.y;