how to create condition using ros odom pose data? - c

Question

I have a mobile autonomous robot that moves using Ros. My goal is to communicate with rosserial via arduino and subscribe to the odom topic. I would like to put some conditions on the exposure data I received after I became a member of Odom. In order to be able to perform some operations at certain points in the coordinates on my map. with a simple example
if (pose_x>2.17 &&pose_x<4.00 &&pose_y<7.6 &&pose_y>9.5 ){
digitalWrite(led_pin, HIGH - digitalRead(13));
}
I want to add conditions like but I don't know how to do that. I'm not very good at software.Below is the code I am trying to do with arduino. I may have been a little silly. I'm still very lacking. I will be grateful for all your help.
#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

It seems like you're not using the data from the odomCallback at all.
Looks like pose_x and pose_y values never get set. Try adding this to the beginning of the odomCallback function (not tested):
// 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;