How to send multiple messeage to arduino to move servos?

edit

Hey, guys! I'm a beginner of ROS and programing.

My probrem is that I can't move two servos using arduino.

I want to send two messeages servo1,servo2 to arduino.

servo1_pub = nh.advertise<std_msgs::UInt16>("servo1",1000);
servo2_pub = nh.advertise<std_msgs::UInt16>("servo2",1000);

I could confirm these are published. So, itried to send using rostopic pub.

$ rostopic pub servo std_msgs/UInt16MultiArray '{data: [servo1, servo2]}'

However, error occures when start servo1 and servo2 publish node.

~$  rostopic pub servos std_msgs/UInt16MultiArray '{data:[servo1,servo2]}'
publishing and latching message. Press ctrl-C to terminate
[WARN] [WallTime: 1541260030.242404] Inbound TCP/IP connection failed: field data[] must be unsigned integer type

Please tell me if someone know about it.

Edit: Thank you for your reply!

I don't know how to define the number of dimensions of the array data from command line.

The arudino sketch is like this.

Arduino:

#if (ARDUINO >= 100)
 #include <Arduino.h>
#else
 #include <WProgram.h>
#endif

#include <Servo.h> 
#include <ros.h>
#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"

#include "std_msgs/UInt16MultiArray.h"

ros::NodeHandle  nh;

Servo servo1;
Servo servo2;

void servo_cb( const std_msgs::UInt16MultiArray&  cmd_msg){
  servo1.write(cmd_msg.data[0]); //set servo angle, should be from 0-180  
  servo2.write(cmd_msg.data[1]); 
  digitalWrite(13, HIGH-digitalRead(13));  //toggle led  
}


ros::Subscriber<std_msgs::UInt16MultiArray> sub("servos", servo_cb);

void setup(){
  pinMode(13, OUTPUT);

  nh.initNode();
  nh.subscribe(sub);


  servo1.attach(9); //attach it to pin 9
  servo2.attach(10);//attach it to pin10
}

void loop(){
  nh.spinOnce();
  delay(1);
}

publish node:

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

#include <iostream>

#include "autoware_config_msgs/ConfigTwistFilter.h"
#include <std_msgs/UInt16.h>

namespace {

//Publisher
ros::Publisher g_twist_pub;
ros::Publisher servo1_pub;
ros::Publisher servo2_pub;
double g_lateral_accel_limit = 5.0;
double g_lowpass_gain_linear_x = 0.0;
double g_lowpass_gain_angular_z = 0.0;
constexpr double RADIUS_MAX = 9e10;
constexpr double ERROR = 1e-8;
 uint16_t servo1 = 0;
  uint16_t servo2 = 0;

void configCallback(const autoware_config_msgs::ConfigTwistFilterConstPtr &config)
{
  g_lateral_accel_limit = config->lateral_accel_limit;
  ROS_INFO("g_lateral_accel_limit = %lf",g_lateral_accel_limit);
  g_lowpass_gain_linear_x = config->lowpass_gain_linear_x;
  ROS_INFO("lowpass_gain_linear_x = %lf",g_lowpass_gain_linear_x);
  g_lowpass_gain_angular_z = config->lowpass_gain_angular_z;
  ROS_INFO("lowpass_gain_angular_z = %lf",g_lowpass_gain_angular_z);
}

void TwistCmdCallback(const geometry_msgs::TwistStampedConstPtr &msg)
{

  double v = msg->twist.linear.x;
  double omega = msg->twist.angular.z;

  if(fabs(omega) < ERROR){
    g_twist_pub.publish(*msg);
    return;
  }

  double max_v = g_lateral_accel_limit / omega;

  geometry_msgs::TwistStamped tp;
  tp.header = msg->header;

  double a = v * omega;
  ROS_INFO("lateral accel = %lf", a);


  tp.twist.linear.x = fabs(a) > g_lateral_accel_limit ? max_v
                    : v;
  tp.twist.angular.z = 180-(1.57+omega)*180/3.14;

  static double lowpass_linear_x = 0;
  static double lowpass_angular_z = 0;
  lowpass_linear_x = g_lowpass_gain_linear_x * lowpass_linear_x + (1 - g_lowpass_gain_linear_x) * tp.twist.linear.x;
  lowpass_angular_z = g_lowpass_gain_angular_z * lowpass_angular_z + (1 - g_lowpass_gain_angular_z) * tp.twist.angular.z;

 if (lowpass_angular_z > 100 || lowpass_angular_z < 80)
  {
    tp.twist.linear.x = 0.2 ;
  }
  else
  {
    tp.twist.linear.x = lowpass_linear_x;
  }


  tp.twist.angular.z = lowpass_angular_z;

  // Controlled servo moto based on expected values
  if (tp.twist.linear.x <0.04)
  {
    servo1 = 90;
  }
  else if (tp.twist.linear.x >= 0.04 && tp.twist.linear.x < 1.0)
  {
    servo1 = 45;
  }
  else
  {
    servo1 = 0;
  }

  servo2 = tp.twist.angular.z;
  ROS_INFO("v: %f -> %f",v,tp.twist.linear.x);
  ROS_INFO("%d ...