stack smashing detected
I use the joystick Joy package to generate joy messages and process them with a node I wrote. My node dies consistently after processing one message and I can't make any sense of what is going on.
Judging by the debug messages I inserted in the code, it looks like the callback is entered once, runs properly and then crashes at some point between its end and its next call.
Any help is greatly appreciated.
Here is the code:
#include <ros/ros.h>
#include <sensor_msgs/Joy.h>
#include <cstdlib>
#include <ros/console.h>
#include <last_letter_msgs/SimPWM.h>
class JoyConverter{
public:
ros::Subscriber sub;
ros::Publisher pub;
int axisIndex[11];
int buttonIndex[11];
double throwIndex[11];
int mixerid;
JoyConverter(ros::NodeHandle n);
~JoyConverter();
void joy2chan(sensor_msgs::Joy joyMsg);
last_letter_msgs::SimPWM mixer(double * input, int mixerid);
};
JoyConverter::JoyConverter(ros::NodeHandle n)
{
sub = n.subscribe("joy",1,&JoyConverter::joy2chan,this);
pub = n.advertise<last_letter_msgs::SimPWM>("rawPWM",1); // Read the controller configuration parameters from the HID.yaml file
XmlRpc::XmlRpcValue listInt, listDouble;
int i;
if(!ros::param::getCached("/HID/throws", listDouble)) {ROS_FATAL("Invalid parameters for -/HID/throws- in param server!"); ros::shutdown();}
for (i = 0; i < listDouble.size(); ++i) {
ROS_ASSERT(listDouble[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
throwIndex[i]=listDouble[i];
}
ROS_INFO("Reading input axes");
if(!ros::param::getCached("/HID/axes", listInt)) {ROS_FATAL("Invalid parameters for -/HID/axes- in param server!"); ros::shutdown();}
for (i = 0; i < listInt.size(); ++i) {
ROS_ASSERT(listInt[i].getType() == XmlRpc::XmlRpcValue::TypeInt);
axisIndex[i]=listInt[i];
}
ROS_INFO("Reading input buttons configuration");
if(!ros::param::getCached("/HID/buttons", listInt)) {ROS_FATAL("Invalid parameters for -/HID/buttons- in param server!"); ros::shutdown();}
for (i = 0; i < listInt.size(); ++i) {
ROS_ASSERT(listInt[i].getType() == XmlRpc::XmlRpcValue::TypeInt);
buttonIndex[i]=listInt[i];
}
// Read the mixer type
if(!ros::param::getCached("/HID/mixerid", mixerid)) {ROS_INFO("No mixing function selected"); mixerid=0;}
}
JoyConverter::~JoyConverter()
{
}
void JoyConverter::joy2chan(sensor_msgs::Joy joyMsg)
{
ROS_DEBUG("joy2chan: Processing new joy msg");
last_letter_msgs::SimPWM channels;
double input[11];
int i;
for (i = 0; i <= 11; i++) {
if (axisIndex[i] != -1) { // if an axis is assigned in this channel
input[i] = 1.0/throwIndex[i]*joyMsg.axes[axisIndex[i]];
}
else if (buttonIndex[i] != -1) {
input[i] = 1.0/throwIndex[i]*joyMsg.buttons[buttonIndex[i]];
}
else {
input[i] = 0.0;
}
}
ROS_DEBUG("joy2chan: Calling mixing function");
channels = mixer(input, mixerid);
for (i=0;i<11;i++) // Cap channel limits
{
if (channels.value[i]<1000) channels.value[i]=1000;
if (channels.value[i]>2100) channels.value[i]=2100;
}
channels.header.stamp = ros::Time::now();
ROS_DEBUG("joy2chan: Publishing channels");
pub.publish(channels);
ROS_DEBUG("joy2chan: Callback ended");
}
// Mixer function
last_letter_msgs::SimPWM JoyConverter::mixer(double * input, int mixerid)
{
last_letter_msgs::SimPWM channels;
int i;
switch (mixerid)
{
case 0: // No mixing applied
for (i=0;i<11;i++)
{
channels.value[i] = (unsigned int)(input[i]*500 + 1500);
}
return channels;
case 1: // Airplane mixing
channels.value[0] = (unsigned int)(input[0]*500+ 1500); // Aileron channel
channels.value[1] = (unsigned int)(input[1]*500+ 1500); // Elevator channel
channels.value[2] = (unsigned int)((input ...
My initial suspicion is that the problem might be with accessing past the end of the
input
array. What happens if you change the firstfor
loop injoy2chan
to have a conditional statement ofi<11
instead ofi<=1
?