将一个主题中的数据放入 Callback 函数中的动态数组中并进行一些计算
Put data from one topic into a dynamic array in the Callback function and do some calculation
我正在从Leap Motion传感器读取我的手部姿势,我想计算手在X
方向上的移动速度(通过计算derivativex = dx / dt
)。我的解决方案是将 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;
}
}
问题 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 附加到矢量 ?你应该使用 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
相关文章:
- 第 i 个元素返回 i 的函数数组
- 使用嵌套函数数组是个好主意吗?
- 正确实现类函数数组
- 试图引用已删除函数数组的相等运算符
- 如何在静态函数中使用成员函数数组
- 无法创建函数数组
- 构造函数数组
- 使用开关或定义函数数组 - 更快
- 编写一个函数,该函数将覆盖主函数数组中的值
- 可以像'funs[1]();'一样调用的函数数组
- std::AVX内部函数数组
- 如果数组大小未知,如何将函数数组作为参数传递
- 从函数数组中获取值
- 使用元编程在编译时初始化函数数组
- 将C函数数组转换为C++函数指针数组
- 复制构造函数 [数组] C++
- 添加函数数组并随机选择其中一个
- 指针到私有成员函数数组
- 可以单独声明函数数组的元素
- (c++)将main中的随机值保存到函数数组中