Ask Your Question
0

How to send multiple messeage to arduino to move servos?

asked 2018-11-03 11:16:18 -0600

shibaneko gravatar image

updated 2018-11-04 01:54:55 -0600

gvdhoorn gravatar image

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

Comments

Thanks for the updates, I've edited my answer now.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2018-11-04 04:32:15 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-11-03 16:07:07 -0600

updated 2018-11-04 04:31:51 -0600

Okay, there are few things I could suggest. Can you show us the code you're running on your arduino. Have you updated this so that it's listening on the servo topic for std_msgs::Uint16MultiArray messages? Is the error being produced by the rosserial node or the rostopic pub node?

All the MultiArray messages include the layout information which describes the number of dimensions of the arrayed data. You need to specify this on the command line as well.

Hope this helps.

UPDATE:

I think I've spotted something, if you're running exactly the following command:

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

Are you trying to publish the contents of the two topics servo1 & servo2 as a MultiArray message? If so that's not how this works. The UInt16MultiArray stores an array of base Uint16 values not UInt16 messages, so you need to give it literal values as shown below:

~$  rostopic pub servos std_msgs/UInt16MultiArray '{data:[400, 300]}'

You can try this quickly from the command line to see if it works. To make this work with your publish node too you'll need to modify it so it publishes a single std_msgs/UInt16MultiArray message containing the two values instead of two separate UInt16 messages.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2018-11-03 11:16:18 -0600

Seen: 108 times

Last updated: Nov 04 '18