ROSからM5stackにふれる その２：Lチカもといsubscriber

実際のところ

#include <ros.h>
#include <std_msgs/Empty.h>
#include <M5Stack.h>


ros::NodeHandle  nh;
bool flag = true;

void messageCb( const std_msgs::Empty& toggle_msg){
  if ( flag ) {
    M5.Lcd.fillScreen(WHITE);
    flag = false;
  } else {
    M5.Lcd.fillScreen(BLACK);    
    flag = true;
  }
}

ros::Subscriber<std_msgs::Empty> sub("toggle_led", &messageCb );

void setup()
{ 
  // initialize the M5Stack object
  M5.begin();
  M5.Power.begin();
  
  M5.Lcd.setCursor(10, 10);
  M5.Lcd.setTextColor(RED);
  M5.Lcd.setTextSize(1);
  M5.Lcd.printf("ROS Test!");

  nh.initNode();
  nh.subscribe(sub);
}

void loop()
{  
  nh.spinOnce();
  M5.update();
  delay(1);
}

ポートがCOM6だったので、WSLから触れるにはttyS6

$ rosrun rosserial_python serial_node.py /dev/ttyS6
[INFO] [1647412156.660886]: ROS Serial Python Node
[INFO] [1647412156.680542]: Connecting to /dev/ttyS6 at 57600 baud
[INFO] [1647412158.860618]: Requesting topics...
[INFO] [1647412158.894693]: Note: subscribe buffer size is 512 bytes
[INFO] [1647412158.898266]: Setup subscriber on toggle_led [std_msgs/Empty]

ようやく投稿

$ rostopic pub toggle_led std_msgs/Empty --once
publishing and latching message for 3.0 seconds

参考もと

rosserial_arduino/Tutorials/Blink - ROS Wiki