Error in serial communication between ROS and Arduino Mega

edit

I installed rosserial_arduino to do the serial communication with ROS (installed on Raspberry Pi 3, Ubuntu 16.04 , ROS Kinetic). The problem is that my output is not correct (it publishes the output but wrong value) so I'm wondering if I made some mistake in the Arduino code.

In the Arduino code, I need to publish 2 analog inputs sensor_msg1 and sensor_msg2 (each having 3 values). I choose the default UInt16MultiArray type because it can represent the int number of analog input (0 to 1023) and it can be represented as an array.

Arduino Code:

#include <ros.h>
#include <std_msgs/UInt16MultiArray.h>

ros::NodeHandle  nh;

std_msgs::UInt16MultiArray sensor_msg1;
std_msgs::UInt16MultiArray sensor_msg2;

ros::Publisher pub_hypcube1("/sensor_msg1", &sensor_msg1);
ros::Publisher pub_hypcube2("/sensor_msg2", &sensor_msg2);

uint16_t output1[3];
uint16_t output2[3];
int mod_11k = 12; 
int mod_17k = 13; 


void setup() {
  pinMode(mod_11k,OUTPUT);  
  pinMode(mod_17k,OUTPUT);
  digitalWrite(mod_11k,LOW);
  digitalWrite(mod_17k,LOW);
  nh.getHardware()->setBaud(115200);
  nh.initNode();

  sensor_msg1.layout.dim = (std_msgs::MultiArrayDimension *)
  malloc(sizeof(std_msgs::MultiArrayDimension)*2);
  sensor_msg1.layout.dim[0].label = "hypcube1";
  sensor_msg1.layout.dim[0].size = 3;
  sensor_msg1.layout.dim[0].stride = 1;
  sensor_msg1.layout.data_offset = 0;
  sensor_msg1.data = (int *)malloc(sizeof(int)*6);
  sensor_msg1.data_length = 3;

  sensor_msg2.layout.dim = (std_msgs::MultiArrayDimension *)
  malloc(sizeof(std_msgs::MultiArrayDimension)*2);
  sensor_msg2.layout.dim[0].label = "hypcube2";
  sensor_msg2.layout.dim[0].size = 3;
  sensor_msg2.layout.dim[0].stride = 1;
  sensor_msg2.layout.data_offset = 0;
  sensor_msg2.data = (int *)malloc(sizeof(int)*6);
  sensor_msg2.data_length = 3;

  nh.advertise(pub_hypcube1);
  nh.advertise(pub_hypcube2);
}


void loop()
{
    tick();
}


void tick() {
    digitalWrite(mod_11k,HIGH);
    delayMicroseconds(4500);
    output1[0] = analogRead(0);   //phi1 11k
    output1[1] = analogRead(1); //phi2 11k
    output1[2] = analogRead(2);
    digitalWrite(mod_11k,LOW);
    delayMicroseconds(200);
    digitalWrite(mod_17k,HIGH);
    delayMicroseconds(4500);
    output2[0] = analogRead(0);
    output2[1] = analogRead(1);
    output2[2] = analogRead(2);
    digitalWrite(mod_17k,LOW);
    delayMicroseconds(200);
    sensor_msg1.data[0] = output1[0];
    sensor_msg1.data[1] = output1[1];
    sensor_msg1.data[2] = output1[2];
    sensor_msg2.data[0] = output2[0];
    sensor_msg2.data[1] = output2[1];
    sensor_msg2.data[2] = output2[2];
    pub_hypcube1.publish(&sensor_msg1);
    pub_hypcube2.publish(&sensor_msg2);
    nh.spinOnce();
}

If anyone find the strange things in the code that may affect the result, please explain to me. Thank you so much