A ROS node to control servos on a PCA9685 board
The primary purpose of this project is to develop a ROS node capable of controlling servos for steering and throttle, such as would be used by a Donkey Car, using the standard Adafruit PCA9685 board, controlled over the I2C bus from a Raspberry Pi.
ROS, obviously. If you don't want to suffer the pain of trying to build ROS on the standard RPi Linux distros, you might look into this tasty image, with ROS pre-installed, from Ubiquity Robotics.
This project also depends on the wiringPi library for I2C communication. It may already be present on your Pi. You can learn more about it here.
On your Pi, once you've verified wiringPi has been installed, create a Catkin workspace directory and clone this repository into src
.
mkdir -p ~/catkin_ws/src
cd src
git clone git@github.com:liamondrop/ros-pca9685-board.git
You should now be able to build with Catkin.
cd ~/catkin_ws
catkin_make
source devel/setup.bash
In the launch file, there is an example servo configuration. This will set the parameters for the steering and throttle servos, as well as the pwm frequency. You may need to change these for your particular setup.
One note on throttle servos for RC cars: when switching from forward to reverse, it is actually necessary to send a reverse pulse twice before the servo will respond.
You may also set the PWM Frequency in the configuration. It defaults to 50hz in this project, which is the standard recommended frequency for servos. If you change this value, you will likely need to update your servo configurations as well.
Once you haveLaunch the pca9685_board
node.
roslaunch pca9685_board pca9685.launch
The pca9685_board
node listens to two topics:
/servo_absolute
: this topic accepts a pca9685_board::Servo
message, which is built by this project. The main purpose of this topic is to send pwm signals to help you fine tune the configuration of your servos. The following messages are an example of using this topic to find the pulse value corresponding to the steering servo's center, i.e. 333.
rostopic pub servo_absolute pca9685_board/Servo "{name: steering, value: 300}"
rostopic pub servo_absolute pca9685_board/Servo "{name: steering, value: 350}"
rostopic pub servo_absolute pca9685_board/Servo "{name: steering, value: 330}"
rostopic pub servo_absolute pca9685_board/Servo "{name: steering, value: 335}"
rostopic pub servo_absolute pca9685_board/Servo "{name: steering, value: 333}"
/servos_drive
: this topic is intended for control and accepts a standard geometry_msgs::Twist
message, using the linear.x
value for the throttle and the angular.z
value for the steering. Note that these values should be between -1.0 and 1.0 inclusive.
rostopic pub servos_drive geometry_msgs/Twist "{linear: {x: 0.5}, angular: {z: -0.75}}"
If you have completed the TeleopTurtle Joystick tutorial, you could control the servos with a joystick by simply relaying the /turtle1/cmd_vel
topic to the /servos_drive
topic.
rosrun topic_tools relay /turtle1/cmd_vel /servos_drive
Note that the angular and linear scales in that project are set to 2, and you will probably want to change them to 1 so the joystick output only ranges from -1.0 to 1.0, as required by this module.
PRs accepted.
Small note: If editing the README, please conform to the standard-readme specification.
MIT © 2018 Liam Bowers