0

我有一个使用 Ros 移动的移动自主机器人。我的目标是通过 arduino 与 rosserial 交流并订阅 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();
}
4

1 回答 1

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;
于 2021-12-09T11:04:30.663 回答