How to Publish Array Data for Multiple Servos on Arduino

asked 2016-05-24 19:58:34 -0500

d7x gravatar image

updated 2020-06-28 22:01:45 -0500

jayess gravatar image

I'm trying to publish an array in Python that is subscribed to by code on an Arduino that has callbacks for an Adafruit PWM servo controller. The code on the Arduino compiles and uploads to an Arduino Uno just fine. I'm not sure how to publish a multiarray for the following example: 3 servos that each have different integer angles between 0 and 180. I'm getting the following error messages when I run:

TypeError: Invalid number of arguments, args should be ['layout', 'data'] args are('{data: [20,50,100]}',)

How should I be publishing the multiarray in this example?

Python Publishing Code:

print "reco_event_servo_pwm: set up data values for servos"
servo_pub1 = rospy.Publisher('servo_pwm', UInt16MultiArray, queue_size=10)

n = 3
while n >= 0:
    servo_pub1.publish('{data: [20,50,100]}')  # THIS IS THE STATEMENT THAT HAS ERRORS
    rate.sleep()
    n = n - 1

Arduino Code:

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

#include <Servo.h>
#include <ros.h>
#include <std_msgs/UInt16MultiArray.h>
#include <std_msgs/String.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>


/////////////////////////////////////////////////////////////////////////////////


ros::NodeHandle  nh;

// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

// Depending on your servo make, the pulse width min and max may vary, you 
// want these to be as small/large as possible without hitting the hard stop
// for max range. You'll have to tweak them as necessary to match the servos you
// have!
#define SERVOMIN  200 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  400 // this is the 'maximum' pulse length count (out of 4096)

// our servo # counter
uint8_t servonum = 0;

void servo_ctlr_cb( const std_msgs::UInt16MultiArray&  cmd_msg) {
//  servo1.write(cmd_msg.data); //set servo angle, should be from 0-180

  for (int i=0; i<3; i++) { // run for all servos for TESTing
    pwm.setPWM(i, 0, cmd_msg.data[i]); //PWM signal, state where it goes high, state where it goes low, 0-4095, deadband 351-362
  }
}

ros::Subscriber<std_msgs::UInt16MultiArray> sub1("servo_pwm", servo_ctlr_cb);

void setup() {
  Serial.begin(9600);

  nh.initNode();
  nh.subscribe(sub1);

  pwm.begin();

  pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates

  for(int j=0;j<8;j++) {  //initialize every thruster via channel (0-7) with a for-loop 
    pwm.setPWM(j,0,351);
  }

}

// you can use this function if you'd like to set the pulse length in seconds
// e.g. setServoPulse(0, 0.001) is a ~1 millisecond pulse width. its not precise!
void setServoPulse(uint8_t n, double pulse) {
  double pulselength;

  pulselength = 1000000;   // 1,000,000 us per second
  pulselength /= 60;   // 60 Hz
  pulselength /= 4096;  // 12 bits of resolution
  pulse *= 1000;
  pulse /= pulselength;
  pwm.setPWM(n, 0, pulse);
}

void loop() {
  nh.spinOnce();
  delay(20);
}
edit retag flag offensive close merge delete

Comments

Hi there. Did you find a solution? I am facing something similar... I need to call the instruction to publish data from another void function but it seems that the python subscriber is not printing anything and with rqt_graph I can see that the rosserial is publishing and python node is subscribing... but I can't see anything In the screen rospy loginfo

subarashi gravatar image subarashi  ( 2020-06-28 14:36:50 -0500 )edit

@subarashi I've moved your answer to a comment. Please remember that this is not a forum. Answers should be answers and anything else should really be a comment.

jayess gravatar image jayess  ( 2020-06-28 22:02:49 -0500 )edit

Bro, thanks but if you won't help me to figure out my problem why are taking time to move my comment LOL

subarashi gravatar image subarashi  ( 2020-06-29 05:51:40 -0500 )edit