Robotics StackExchange | Archived questions

Looking for maximal value of a published ROS message ( store data using table )

Hello everyone, I'm trying to subscribe to a ROS topic ( float) and looking to store the published data in a table and then to look for the maximal value inside the table. anyone could help me to do it. My objective is to determine the maximal value among the published data. If anyone has another idea rather than using table, i'm interested also.

#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Bool.h> 
#include <iostream>
#include <vector>
#include <ros/console.h>
#include <stdio.h>
using namespace std;
//Variable declaration
std_msgs::Float64 distanceFollowerLeader;
std_msgs::Bool startObserver;
std_msgs::Float64 maxDistance;
float ActualDistance;
vector<float> maxima(100);
vector<float> tableau (100);

//Callback function
void myCallbackObserver(const std_msgs::Bool& message_holder)
{
    ROS_INFO("received Observer restart:%d",message_holder.data);
    startObserver.data=message_holder.data;
}
void myCallbackdistanceLeaderFollower(const std_msgs::Float64& message_holder)
{
    //check for data on topic "distanceFollowerLeader"
    ROS_INFO("received distanceLeaderFollower value is:%f",message_holder.data);
    distanceFollowerLeader.data=message_holder.data;
}

//main program
int main(int argc, char **argv)
{
    ros::init(argc,argv,"ObserverNode");//name this node
    ros::NodeHandle nh; // node Handle    

//create subscribers
    ros::Subscriber my_subscriber_object1=nh.subscribe("startObserver",1,myCallbackObserver);
    ros::Subscriber my_subscriber_object2=nh.subscribe("distanceFollowerLeader",1,myCallbackdistanceLeaderFollower);

// create publishers 
    ros::Publisher my_publisher_object2=nh.advertise<std_msgs::Float64>("maxDistance",1);
    ros::Rate loop_rate(1);
    startObserver.data=true;
    maxDistance.data = 10;
    while(ros::ok()) {
       if (startObserver.data) {

        maxima.push_back(maxDistance.data);
        ActualDistance=distanceFollowerLeader.data;
       if (ActualDistance > maxDistance.data) {
        maxDistance.data = ActualDistance;
        ROS_INFO("max scenario :%f",maxDistance.data);
        }
        startObserver.data=false;
        maxima.push_back(maxDistance.data);
    }
        my_publisher_object2.publish(maxDistance);
        ROS_INFO("max distance =%f",maxDistance.data);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0; //should never get here, unless roscore dies
}

I'm trying fo find the maximal distance. To do so this node subscribe to the topic
. the objective is to find the maximal distance value at the end of the observation. Thank you in advance for your help.

Asked by Hamza on 2018-12-03 04:45:55 UTC

Comments

If you're only interested in finding the maximum value you could simply store the previous maximum and compare new values to that. No need to store a table that potentially could get very large.

Asked by PeteBlackerThe3rd on 2018-12-03 05:11:45 UTC

thank you very much for your reply. i tried to do it but the maximal value did not change and remain constant (initial value) although using the rostopic echo i clearly saw the content of the message changing along the simulation.

Asked by Hamza on 2018-12-03 05:22:22 UTC

There must be a bug in your code in that case. Can you pose your source code for this and we'll get it fixed.

Asked by PeteBlackerThe3rd on 2018-12-03 05:30:47 UTC

I updated the question to include my code. thank you very much for your help.

Asked by Hamza on 2018-12-03 06:02:16 UTC

Answers

Fundamentally two things are wrong with your code which are stopping the maximum value from being found.

You need to move the ros::Spin() inside your main while loop. As it is now, the callbacks can only be executed one time before the loop starts.

Secondly you're currently resetting the value of maxDistance.data to 10 every iteration of your main loop erasing any maximum value found. This initialisation needs to be before the loop.

Asked by PeteBlackerThe3rd on 2018-12-03 06:12:22 UTC

Comments

I try to modify the code as you suggested (update the code above) and it doesn't work. the maximal distance is fixed at 10. thank you again for your help

Asked by Hamza on 2018-12-03 07:53:56 UTC

Your code seems rather strange. When a true value is received on the startObserver topic then the next single distance reading will be checked to see if it's greater than the current maximum, but it will then stop checking until another message is received on startObserver. Is this what you want?

Asked by PeteBlackerThe3rd on 2018-12-03 08:24:21 UTC

My Objective is to measure the maximal distance for a specific period ( this period is indicated by the startObserver or at least the beginning). After that, I will reinitialize my parameters, change context and then recompute the maximal distance (I have also others nodes in my package

Asked by Hamza on 2018-12-03 08:31:54 UTC

inside the if statement your setting startObserver.data=false; so you're only checking a single message to see if it's greater than the maximum. This if should be inside the callback itself not the main loop. As it is If you receive 100 messages a second, you're only checking 1 of them.

Asked by PeteBlackerThe3rd on 2018-12-03 08:44:06 UTC

thank you so much for your help. I commented the startObserver.data=false; and now i notice that max is working. I just did not understand where to put the if. Would you explain haw to put it in oreder to start and to finish the observation ?

Asked by Hamza on 2018-12-03 10:59:38 UTC