ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

stack smashing detected

asked 2016-08-31 13:16:54 -0500

Georacer gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

Comments

My initial suspicion is that the problem might be with accessing past the end of the input array. What happens if you change the first for loop in joy2chan to have a conditional statement of i<11 instead of i<=1?

jarvisschultz gravatar image jarvisschultz  ( 2016-08-31 15:28:06 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2016-09-01 00:57:58 -0500

DavidN gravatar image

There are a few problems with your code, which potentially crashes your node:

  1. Are you sure the joyMsg in the callback joy2chan always has a fixed number of axes and buttons? I saw that you always trying to read 11 (or 12?) elements from the array.

  2. This part of your code is confusing: for (i = 0; i <= 11; i++) ... Are you trying to say that there are 12 elements? Because you only declare double input[11];

edit flag offensive delete link more

Comments

I'm embarrassed to say it, but it was <=11 that was breaking the code. Two weird parts, though: 1) If I was calling an invalid element of input inside joy2chan, why did the callback finish (as seen by the debug messages) and the crash? 2) This code used to run fine in 14.04 and Indigo. Thanks!

Georacer gravatar image Georacer  ( 2016-09-01 01:19:59 -0500 )edit

Perhaps Kinetic uses a new compiler, which is more strict.

Georacer gravatar image Georacer  ( 2016-09-01 01:23:49 -0500 )edit
1

The stack grows downwards from the top of your address space, so writing past the end of an array that's on the stack will clobber whatever else is on the stack. Depending on how the compiler decides to lay out your stack, that could be another variable, the return address or something else.

ahendrix gravatar image ahendrix  ( 2016-09-01 05:15:14 -0500 )edit

I suspect the compiler or the C library is putting a sentinel value between stack frames, and using that to detect if you've accidentally written past the end of your frame. Might be showing up now because it's new, might be due to better compiler optimizations, or just different stack layout.

ahendrix gravatar image ahendrix  ( 2016-09-01 05:17:30 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2016-08-31 13:16:54 -0500

Seen: 2,141 times

Last updated: Sep 01 '16