2788west / cerusbot Goto Github PK

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

//Configure ROS
ros::NodeHandle nh;
geometry_msgs::Twist msg;

//Create a message callback that updates the motor speeds
void messageCb(const geometry_msgs::Twist& cmd_vel)
{
    digitalWrite(5, HIGH-digitalRead(5)); 
}

//Subscribe to the Twist message
ros::Subscriber <geometry_msgs::Twist> sub("/cmd_vel", &messageCb);

//Setup the node and subscriber once
void setup()
{
    nh.initNode();
    nh.subscribe(sub);
    pinMode(5, OUTPUT);
    digitalWrite(5, HIGH);
    Serial.begin(9600); // needs to be at end of setup loop
}

//Loop forever
void loop()
{
  while (!nh.connected())
  {
    nh.spinOnce();
  }
    nh.spinOnce();
    delay(1);
}