openCR-用ROS代码点亮LED的方法

/*
//rostopic pub -1 led_out_kay std_msgs/Byte 1 // 用户1 LED On
//rostopic pub -1 led_out_kay std_msgs/Byte 2 // 用户2 LED On
//rostopic pub -1 led_out_kay std_msgs/Byte 4 // 用户3 LED On
//rostopic pub -1 led_out_kay std_msgs/Byte 8 // 用户4 LED On
//rostopic pub -1 led_out_kay std_msgs/Byte 0 // LED all Off
 */

#include <ros.h>
#include <ros/time.h>

#include <geometry_msgs/Vector3.h>//Subscriber
#include <geometry_msgs/Twist.h>

#include <std_msgs/String.h>//std_msgs
#include <std_msgs/Byte.h>

/*******************************************************************************
  ROS NodeHandle
*******************************************************************************/
ros::NodeHandle nh;

int led_pin_user[4] = { BDPIN_LED_USER_1, BDPIN_LED_USER_2, BDPIN_LED_USER_3, BDPIN_LED_USER_4 };
void hellokay(const std_msgs::Byte& led_msg);

/*******************************************************************************
  Subscriber
*******************************************************************************/
ros::Subscriber<std_msgs::Byte> cmd_kay_sub("led_out_kay", hellokay );//led_out_kay话题，调用终端来控制
//四个LED定义为led_out订阅者

//1:打开一个终端，运行roscore
//2.下载这个程序
//3.再打开另一个终端，运行rosrun rosserial_python serial_node.py __name:=opencr _port:=/dev/ttyACM0 _baud:=115200
//前提：安装了rosserial_python   
//4.再次打开一个终端，运行rotopic list 查看有没有 /led_out_kay这个话题，有说明程序正确
//5.再打开一个终端，运行rostopic pub -1 led_out_kay std_msgs/Byte 1     
//你会发现，第一个用户led灯亮，测试完成


void setup() 
{
  pinMode(led_pin_user[0], OUTPUT);
  pinMode(led_pin_user[1], OUTPUT);
  pinMode(led_pin_user[2], OUTPUT);
  pinMode(led_pin_user[3], OUTPUT);

  nh.initNode();//初始化节点
  nh.subscribe(cmd_kay_sub);
}

void loop() 
{
  nh.spinOnce(); //loop函数中，调用了ros::spinOnce()，在这个函数中，所有ROS通信的回调函数都被处理
}

//如果传递的消息值的0~3位bit值为1，则打开相应的LED，反之关闭相应的LED。
void hellokay(const std_msgs::Byte& led_msg)//ROS标准数据类型std_msg/Byte
{
  int i;
  for (i = 0; i < 4; i++)
  {
    if (led_msg.data & (1 << i))//led_msg.data为参数，参数决定开关，没毛病&&&&&&&&
    {
      digitalWrite(led_pin_user[i], LOW);
    }
    else
    {
      digitalWrite(led_pin_user[i], HIGH);
    }
  }
}

　　图附上：