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

我正在从Leap运动传感器读取我的手姿势，我想计算手在

X

方向上移动的速度（通过计算

DerivativeEx=dx/dt

）。我的解决方案是将100个手姿势值放入一个数组中，当新消息（

msg->palmpos.x

）通过主题

leapmotion/data

到达回调函数时，用新值不断更新该数组

我的问题是，当我打印带有ROS_错误（“Hello%f”，“derivativex”）的

derivativex=dx/dt

时，输出总是：0

我做错了什么？对于我的回调正在侦听的主题

我的回拨功能：

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

#包括“geometry_msgs/TwistStamped.h”
#包括“jog_msgs/JogJoint.h”
#包括“jog_msgs/leapros.h”
#包括“ros/ros.h”
#包括
#包括
#包括
#包括
使用名称空间std；
要扭曲的命名空间
{
类SpaceNavToWist
{
公众：
spaceNavToTwist（）：微调器（1）
{
joy_sub_uuu=n_uuu.subscribe（“leapmotion/data”，1，&spaceNavToTwist:：joyCallback，this）；
//将“spacenav/joy”更改为主题“/leapmotion/data”
twist_pub_uuu=n_uu.advision（“jog_arm_服务器/delta_jog_cmds”，1）；
joint_delta_pub_uu=n_u.advision（“jog_arm_服务器/joint_delta_jog_cmds”，1）；
微调器_uu.start（）；
ros:：WaitForShutton（）；
};
const int arraySize=100；//常量变量可用于指定数组大小
double vectorx[arraySize]={}；//将数组n的元素初始化为0
int分辨率=10；
双导数=0；
双dx=0；
int dt=0；
私人：
ros：：NodeHandle n_；
ros：：订阅者的喜悦；
ros：：出版商twist_pub_，joint_delta_pub_；
ros:：异步微调器微调器；
//将传入的joy命令转换为折弯命令。
void joyCallback（const jog_msgs:：leapros:：ConstPtr&msg）
{ 
对于（int count=0；countpalmpos.x；
如果（计数>分辨率）{
dx=vectorx[count-1]-vectorx[count-（分辨率-1）]；
dt=分辨率；
导数x=dx/dt；
ROS_错误（“Hello%f”，DerivativeEx）；
}    
如果（计数==阵列化）{
计数=0；
}
}

问题1:日志函数ROS\u ERROR被误用。您应该传递一个浮点值而不是字符串，否则，您将得到一个未定义的行为：

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

ROS_错误（“Hello%f”，developerativex）；//palmpos.x；//分辨率）{
dx=vectorx[count-1]-vectorx[count-（分辨率-1）]；//与（msg->palmpos.x-msg->palmpos.x）相同-->0
dt=分辨率；
导数x=dx/dt；
ROS_错误（“Hello%f”，DerivativeEx）；
}    
如果（计数==阵列化）{
count=0；//将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

//将其添加到您的文件中
#包括
//你的程序体。。。
//...
//因为我们使用C++，如果可能的话，尝试使用C++设备。
//const int arraySize=100；//常量变量可用于指定数组大小
//double vectorx[arraySize]={}；//将数组n的元素初始化为0
向量向量向量；
int分辨率=10；
int max_vector_size=100；//在向量中保留100个元素。
//...
//将传入的joy命令转换为折弯命令。
void joyCallback（const jog_msgs:：leapros:：ConstPtr&msg）
{ 
//将x坐标存储在矢量图形中
向后推（味精->手掌位置x）；
如果（vectorx.size（）>分辨率）{
int id_back=vectorx.size（）-1；
双dx=vectorx[id_back]-vectorx[id_back-分辨率]；
双dt=分辨率；
导数x=dx/dt；
ROS_错误（“Hello%f”，DerivativeEx）；
}
while（vectorx.size（）>最大向量大小）{
vectorx.erase（vectorx.begin（））；//删除第一个元素
}
}//eof呼叫
Hi，您说过“输出总是：但是你没有说总是什么？输出总是：0一个ROS程序员说，你回调中的循环正在将VECTROX的每个元素设置为消息中最新的x值，因此你的dx无论如何都将为零。你知道如何修复它吗？嗨，先生，一个ROS程序员在你回调中的循环设置完成时说将VECTROX的每个元素都设置为消息中最近的x值，这样您的dx无论如何都将为零。您知道如何修复它吗？请看下面的答案：我想我的问题更多的是为什么不能将回调函数中接收到的一定数量的不同消息逐个放入数组中？？？我认为这是正常可行的y、 是因为ROS回调函数非常不同吗？@StevenSu：我更新了我的答案，请解释更多关于方法和预期行为的信息。我将解释更多关于我的目的。我使用一个名为leap motion的传感器跟踪我的手，这个传感器可以读取你的手的位置并发送手的位置数据（msg->palmpos）然而，我真正需要的是我的手的移动速度（导数=dx/dt）。
//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