Rosserial + cpp nodes: cannot use data from rosserial [closed]

edit

Hi everyone!

I would like to use ultra sonic sensors mounted on a turtlebot to create an obstacle avoidance feature.

Those sensors are connected on an Arduino Uno and I'm using Rosserial_arduino to pass the data to my ros nodes.

My problem is that I can read the data from the sensors, publish them onto a topic (called /Maxbotix_sensor) and print them in the terminal via rostopic echo /Maxbotix_sensor.

But, I have a problem with the subscriber in my ROS node: only one set of data shows up and then nothing else...

Do you have any ideas why that happened?

Thank you for your advices and your help!

EDIT: I tried again this morning by restarting from nothing> I followed again the tutorials about subcribers/publishers and it's now working!

PS: Here are the codes (really really simple) for:

• Arduino

#include <ros.h>
#include <geometry_msgs/Twist.h>

ros::NodeHandle  nh;

geometry_msgs::Twist msg;
ros::Publisher pub1("Maxbotix_sensor", &msg);

const int anPin1 = 0;
const int anPin2 = 1;
const int anPin3 = 2;
const int anPin4 = 3;
const int anPin5 = 4;
int triggerPin1 = 13;
float distance1, distance2, distance3, distance4, distance5;
float data1, data2, data3, data4, data5;

const int nb_sample_4_average = 1000;

void setup()
{
  nh.initNode();
  nh.advertise(pub1);

  data1=0;
  data2=0;
  data3=0;
  data4=0;
  data5=0;
}

void start_sensor(){
  digitalWrite(triggerPin1,HIGH);
  delay(1);
  digitalWrite(triggerPin1,LOW);
}
void read_sensor(){

  for(int i=0; i<nb_sample_4_average; i++)
  {
    /* Get distance between sensor and object & convert in meters (*0.0254)*/
    distance1 = (analogRead(anPin1)/2)*0.0254;
    distance2 = (analogRead(anPin2)/2)*0.0254;
    distance3 = (analogRead(anPin3)/2)*0.0254;
    distance4 = (analogRead(anPin4)/2)*0.0254;
    distance5 = (analogRead(anPin5)/2)*0.0254;

    /* Filter the values */
    data1 = data1 + distance1;
    data2 = data2 + distance2;
    data3 = data3 + distance3;
    data4 = data4 + distance4;
    data5 = data5 + distance5;
  }
}

void loop()
{
  start_sensor();
  read_sensor();

  /* Put data into the msg*/
  msg.linear.x = data1/nb_sample_4_average;
  msg.linear.y = data2/nb_sample_4_average;
  msg.linear.z = data3/nb_sample_4_average;
  msg.angular.x = data4/nb_sample_4_average;
  msg.angular.y = data5/nb_sample_4_average;</xmp>
  /* RAZ buffers*/
  data1=0;
  data2=0;
  data3=0;
  data4=0;
  data5=0;

  /* Publish the msg */
  pub1.publish( &msg );
  nh.spinOnce();
  delay(1);
}

• my node .cpp

#include "test_sensor_node.hpp"

using namespace std;
using namespace ros;


int main(int argc, char **argv)
{
    ros::init(argc, argv, "test_sensor");
    ObsAvoid Obs;
    ros::spinOnce();
    return 0;
}


void ObsAvoid::obstacle_avoidance(const geometry_msgs::Twist& msg)
{   
    cout<<"New data"<<endl;

    obstacle[0] = msg.linear.x;
    obstacle[1] = msg.linear.y;
    obstacle[2] = msg.linear.z;
    obstacle[3] = msg.angular.x;
    obstacle[4] = msg.angular.y;

    for(int ii=0;ii=4;ii++)
    {cout<<obstacle[ii]<<endl;}
    system("clear");
}

• my node .hpp

#ifndef TEST_SENSOR_NODE_HPP_
#define TEST_SENSOR_NODE_HPP_

#include <ros/ros.h>
#include <iostream>
#include <math.h>
#include <string>
#include <stdio.h>
#include <geometry_msgs/Twist.h>

using namespace std;
using namespace ros;

class ObsAvoid
{
    private:
        ros::NodeHandle n;
        ros::Subscriber sub1;       
    public:
        float obstacle[5];
        ObsAvoid()
        {
            sub1=n.subscribe("Maxbotix_sensor",10,&ObsAvoid::obstacle_avoidance,this);
        }
        ~ObsAvoid()
        {
            for(int i=0;i=4;i++)
            {
                obstacle[i]=1000;
            }
        }
        void obstacle_avoidance(const geometry_msgs::Twist& msg);   
};

#endif