Put data from one topic into an dynamic array in the Callback function and do calculation in ROS

asked 2018-12-21 22:15:27 -0500

stevensu1838
27 ●14 ●17 ●20

updated 2018-12-22 05:09:46 -0500

Hi, I am actually reading my hand pose from a Leap Motion sensor and I want to calculate how fast the hand moves(by calculating derivativex = dx / dt) in X direction. My solution is to put 100 hand pose values in an array and keep updating this array with new values when new messages (msg->palmpos.x) arrive in the callback function through topic leapmotion/data.

My question is when I print the derivativex = dx / dt with ROS_ERROR("Hello %f", "derivativex") the output is always: Can you please kindly tell me what I've been doing wrong? Thanks a miliion Btw, plese find the link for the topic that my callback is listening if it is helpful for you the topic publisher. Thanks a million.

Please take a look at my call back function:

#include "geometry_msgs/TwistStamped.h"
#include "jog_msgs/JogJoint.h"
#include "jog_msgs/leapros.h"
#include "ros/ros.h"
#include <ros/console.h>
#include <iostream>
#include <iomanip>
#include <array>
using namespace std;

namespace to_twist
{
class spaceNavToTwist
{
public:
  spaceNavToTwist() : spinner_(1)


{
    joy_sub_ = n_.subscribe("leapmotion/data", 1, &spaceNavToTwist::joyCallback, this);
    // Changed "spacenav/joy" to topic "/leapmotion/data"
    twist_pub_ = n_.advertise<geometry_msgs::TwistStamped>("jog_arm_server/delta_jog_cmds", 1);
    joint_delta_pub_ = n_.advertise<jog_msgs::JogJoint>("jog_arm_server/joint_delta_jog_cmds", 1);

    spinner_.start();
    ros::waitForShutdown();
  };

  const int arraySize = 100;// constant variable can be used to specify array size
  double vectorx[ arraySize ] = {};// initialize elements of array n to 0
  int resolution = 10;
  double derivativex = 0;
  double dx = 0; 
  int dt = 0;

private:

  ros::NodeHandle n_;
  ros::Subscriber joy_sub_;
  ros::Publisher twist_pub_, joint_delta_pub_;
  ros::AsyncSpinner spinner_;
  // Convert incoming joy commands to TwistStamped commands for jogging.
  void joyCallback(const jog_msgs::leapros::ConstPtr& msg)
 { 
    for ( int count = 0; count < arraySize; ++count ) {// store the values of poses
       vectorx[ count ] = msg->palmpos.x;
       if (count>resolution) {
           dx = vectorx[ count-1 ] - vectorx[ count-(resolution-1) ];
           dt = resolution;
           derivativex = dx / dt;
           ROS_ERROR("Hello %f", derivativex);

       }    

       if (count == arraySize) {
           count=0;  
       }
    }

edit retag flag offensive close merge delete

add a comment

answered 2018-12-22 03:50:02 -0500

PeteBlackerThe3rd

flag of United Kingdom of Great Britain and Northern Ireland

9529 ●30 ●143 ●135 https://www.linkedin.c...

updated 2018-12-22 17:07:11 -0500

The reason the value of derivativex is not being printed out is your ROS_ERROR statement's wrong.

ROS_ERROR("Hello %f", "derivativex");

You're specifying derivativex as a string literal by putting it inside "speech marks" so exactly those characters are being printed out. If you remove the speech marks then it will take the value of derivativex as you want.

ROS_ERROR("Hello %f", derivativex);

However there are more problems with your code you'll need to fix to get it doing exactly what you want. At the moment the loop in your callback is setting every element of vectorx to the most recent x value from the message, so your dx will anyways be zero.

Hope this helps you make some progress.

Update:

I can give you some structural advice so you can update your program so it does what you want. But let me first confirm exactly what is it you're trying to do. You want your program to add each x value of palmpos to an array as they are received. When 100 positions have been received you want to calculate some values, empty the array and start the process again.

Firstly the callback function gets executed once in its entity each time a new palmpos message is received. If you put a loop inside it all of that loop will be executed everytime a new value is received.

You'll need to another class member variable to keep track of how many values have been received so far. Then inside the callback you'll want to add the single new value to the end of the array and increment the new variable which stores the number of messages received.

Then also inside the callback you'll want to check if there are now 100 messages stored. If there are you then want to perform your average calculation and reset the number of values received to zero to restart the progress.

Hope this helps.

edit flag offensive delete link

Comments

Hi Sir, Yeah, it is printing zero. And my purpose is to put the past 100 x-values in the array and do the calculation to get the speed of my hand. When this is done, another round of putting 100 x-values will be started. Can you please explain more how I can get this idea to work？ Thanks a lot

stevensu1838 ( 2018-12-22 04:15:12 -0500 )edit

Let's say the past 10 most recent x values are: 0.1, 0.3, 0.5, 0.7, 1.2, 1.6, 1.9, 2.7, 2.9, 3.1. the derivativex = (3.1- 0.1)/10 = 0.3 I think this way to calculate the speed of my hand looks right. Can you please tell me why it is not working? Thanks

stevensu1838 ( 2018-12-22 04:27:22 -0500 )edit

Hi Pete, I see your point. You are saying if the most recent value is 0.3 all the elements of my array are 0.3. However, I think you've noticed that I actually want the recently past different 100 x-vlaues instead of the only most recent one. Do you know how to do it??? Thanks a lot

stevensu1838 ( 2018-12-22 04:34:19 -0500 )edit

I think my question is more about why can't put a certain number of different messages received one by one within a callback function into an array??? I reckon it is doable normally. Is it because a ROS callback function is very different???

stevensu1838 ( 2018-12-22 05:13:56 -0500 )edit

Hi all, please take a look at the following feedback from a good programmer:

for ( int count = 0; count < arraySize; ++count ) {// store the values of poses

   //Could you please explain why the program needs this ???
   vectorx[ count ] = msg->palmpos.x; // <-- every element in vectorx is set to this values (const in each function call).

   if (count>resolution) {
       dx = vectorx[ count-1 ] - vectorx[ count-(resolution-1) ]; // is the same as (msg->palmpos.x - msg->palmpos.x) --> 0

       dt = resolution;
       derivativex = dx / dt;

       ROS_ERROR("Hello %f", derivativex);
   }    

   if (count == arraySize) {
       count = 0;  //<-- never get here because of count is always lesser than arraySize 
   }

}

stevensu1838 ( 2018-12-22 15:55:45 -0500 )edit

And I explained my questions again as below: I will explain more about my purpose. I am using a sensor called leap motion to track my hand and this sensor can read the position of your hand and send the hand position data( msg->palmpos) to this callback function.

stevensu1838 ( 2018-12-22 15:58:34 -0500 )edit

I simply want to get a number of (in this case 100) different hand postion values(not a same one), then I can select part of these successive numbers to claculate my hand speed(derivativex = dx / dt).

stevensu1838 ( 2018-12-22 16:01:15 -0500 )edit

So I am sure it is right about the mistake I made on this part: vectorx[ count ] = msg->palmpos.x; // <-- every element in vectorx is set to this values (const in each function call). Can anyone tell me how I can store these different values for my use instead of getting this const one only?

stevensu1838 ( 2018-12-22 16:02:25 -0500 )edit

see more comments

Put data from one topic into an dynamic array in the Callback function and do calculation in ROS

1 Answer

Comments

Question Tools

Stats

Related questions

Put data from one topic into an dynamic array in the Callback function and do calculation in ROS edit

1 Answer

Comments

Question Tools

Stats

Related questions

Put data from one topic into an dynamic array in the Callback function and do calculation in ROS