Поместите данные из одной темы в динамический массив в функции обратного вызова и выполните некоторые вычисления

Question

Я читаю позу своей руки с датчика Leap Motion и хочу вычислить, как быстро движется рука (вычисляя derivativex = dx / dt) в направлении X.Мое решение состоит в том, чтобы поместить 100 значений поз вручную в массив и продолжать обновлять этот массив новыми значениями, когда новые сообщения (msg->palmpos.x) поступают в функцию обратного вызова через тему leapmotion/data.

Мой вопрос: когда явыведите derivativex = dx / dt с ROS_ERROR("Hello %f", "derivativex"), вывод всегда будет: 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;  
       }
    }

Answer 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 к вектору x?Вы должны использовать 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