将一个主题的数据放入回调函数中的动态数组中并进行一些计算

Posted 2023-02-21

【中文标题】将一个主题的数据放入回调函数中的动态数组中并进行一些计算

【英文标题】：Put data from one topic into a dynamic array in the Callback function and do some calculation

【发布时间】：2018-12-22 04:24:13

【问题描述】：

我正在从 Leap Motion 传感器读取我的手部姿势，我想计算手部在 X 方向上的移动速度（通过计算 derivativex = dx / dt）。我的解决方案是将 100 个手势值放入一个数组中，并在新消息 (msg->palmpos.x) 通过主题 leapmotion/data 到达回调函数时使用新值不断更新此数组。

我的问题是当我用ROS_ERROR("Hello %f", "derivativex") 打印derivativex = dx / dt 时，输出总是：0

我做错了什么？ link 我的回调正在收听的主题。

我的回调函数：

#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;  
       
    

【问题讨论】：

嗨，你说“输出总是：”但你没有说它总是什么？

输出始终为：0 一位 ROS 程序员说目前您的回调中的循环正在将 vectorx 的每个元素设置为消息中最近的 x 值，因此您的 dx 无论如何都会为零。你知道如何解决吗？ ——

【参考方案1】：

问题1：日志函数ROS_ERROR被滥用。你应该传递一个浮点数而不是一个字符串，否则，你会得到一个未定义的行为：

       ROS_ERROR("Hello %f", derivativex); // <-- there is no double quotes.

问题2：X的导数始终为0，因为for循环开始时的赋值：

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 
       

我猜你想将 msg->palmpos.x 附加到 vectorx ？你应该使用 std::vector 作为vectorx，它会容易得多。

这是你的程序的修改版本，使用 std::vector ：

//add this to your file
#include <vector>

//Your program body ...
//...

//As we are using C++, try to use C++ facilities if possible.
//const int arraySize = 100;// constant variable can be used to specify array size
//double vectorx[ arraySize ] = ;// initialize elements of array n to 0

std::vector<double> vectorx;

int resolution = 10;
int max_vector_size = 100; //keep 100 elements in the vectorx.
//...

// Convert incoming joy commands to TwistStamped commands for jogging.
void joyCallback(const jog_msgs::leapros::ConstPtr& msg)
 
     //store the x coordinate in the vectorx
     vectorx.push_back( msg->palmpos.x );

     if( vectorx.size() > resolution )
         int id_back = vectorx.size() - 1;
         double dx = vectorx[id_back] - vectorx[ id_back - resolution ];
         double dt = resolution;

         derivativex = dx / dt;
         ROS_ERROR("Hello %f", derivativex);
     

     while(vectorx.size() > max_vector_size ) 
         vectorx.erase( vectorx.begin() ); //remove the first element
     

//eof joyCallback

【讨论】：

您好，先生，一位 ROS 程序员说目前您的回调中的循环正在将 vectorx 的每个元素设置为消息中最近的 x 值，因此您的 dx 无论如何都会为零。你知道怎么解决吗？

请看这个答案：answers.ros.org/question/311345/…

我想我的问题更多是关于为什么不能将回调函数中收到的一定数量的不同消息一一放入数组中？我认为这通常是可行的。是不是因为一个ROS回调函数很不一样？？？

@StevenSu：我更新了我的答案，请详细说明方法和预期行为。

我会详细解释我的目的。我正在使用一个名为跳跃运动的传感器来跟踪我的手，这个传感器可以读取你的手的位置并将手的位置数据（msg->palmpos）发送到这个回调函数。但是，我真正需要的是手部运动的速度（导数x = dx / dt）。