How to send multiple messeage to arduino to move servos?
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 ...
Thanks for the updates, I've edited my answer now.