Wheel lights

[RobertBlakeAnderson]

I'm working on a feature to control the Pioneer wheel lights. Here's my service callback. It's based on the Aria example mtxWheelLights.cpp

bool RosArnlNode::wheel_light_cb(rosarnl::WheelLight::Request& request, rosarnl::WheelLight::Response& response)
{
  // Validate input
  if (request.mode < 1 || request.mode > 10 || request.value < 0 || request.value > 100) {
    return false;
  }

  struct {
    ArTypes::UByte pattern;
    ArTypes::Byte value;
    ArTypes::UByte flags;
    ArTypes::UByte flags2;
  } msg;

  msg.pattern = request.mode;
  msg.value = request.value;
  msg.flags = 0;
  msg.flags2 = 0;

  arnl.robot->comDataN(ArCommands::WHEEL_LIGHT, (const char*)&msg, 4);

  return true;
}

When I call the service, the wheel lights will flash the commanded mode for an instant, then go back to Ready mode. Is there something else I need to do to make the command stick?

[RobertBlakeAnderson]

After looking at it some more, I noticed function void RobotMonitor::robotMonitorTask() in RobotMonitor.cpp. This function appears to be called continuously and sets the wheel lights based on the robot state. Seems that this is overwriting my command almost immediately.

I'll look into a way to toggle the lights between the default behavior and manual command mode.

[reed-adept]

Maybe the way to do it is to have the service call something in RobotMonitor to set the desired wheel light state, or to disable/enable RobotMonitor's wheel light code. RobotMonitor also puts a popup dialog box in MobileEyes when the robot is e-stopped which gives you a button to re-enable the motors, though in ROS you could also use the enable motors service. The intention for RobotMonitor is to (depending on requirements from users) potentially be a starting point for more sophisticated API for customizing responses to various robot events or state changes that could happen alongside "normal" operation... so any ideas would be appreciated. The RobotMonitor in rosarnl and rosaria are just copies of the same class in the ARNL example server, but if it seems like it has a stable API/design it will be moved into the ARIA library itself.

[RobertBlakeAnderson]

I think I'll need to control the lights via ros-arnl, since the robot and environment states that we want to signal are only visible on the client side. I'm thinking:

• Give the wheel lights a manual operating mode, where that portion of robotMonitorTask() is disabled, and a default mode where it is active. Toggle this with a boolean member in RobotMonitor.
• Create a public function in RobotMonitor to set this bool true/false.
• Create a public function in ArnlSystem as well to expose it to ros-arnl.
• Implement the service callback above, and add to it a function call to set the lights to manual mode, so the command doesn't get overridden.
• Implement a mode in the ROS service to set the wheel lights back to automatic control. Or maybe just have them go back after a certain time. Not sure yet on that.

[reed-adept]

That sounds pretty good. The wheel light states are enumerated by an 8-bit value, then some of the states also look at two other bytes in the command for additional info to modulate color or speed or whatever. There is no state for 0 so that could mean "default" (let RobotMonitor do it) in the service call or topic that changes it. Or use a signed int and use -1 for default (or a signed char, there aren't and won't be more than a dozen possible states.) That could make the interface simpler. Note that this stuff will only apply to the LX at this point. See the robot manual for a list of the states.

Alternatively, we could let RobotMonitor override the state requested by the client in its few cases (e-stop and motors (still) disabled.)

[RobertBlakeAnderson]

I've gotten it working along those lines, with 0 indicating the default behavior. The service definition includes constants to make it user friendly. If you want, I can make a pull request once #19 is dispositioned.

bool RosArnlNode::wheel_light_cb(rosarnl::WheelLight::Request& request, rosarnl::WheelLight::Response& response)
{
  // Validate input
  if (request.mode < 0 || request.mode > 10 || request.value < 0 || request.value > 100) {
    return false;
  }

  if (request.mode == rosarnl::WheelLightRequest::AUTO) {
    arnl.monitor->setWheelLightDefaultMode(true);
    return true;
  }

  struct {
    ArTypes::UByte pattern;
    ArTypes::Byte value;
    ArTypes::UByte flags;
    ArTypes::UByte flags2;
  } msg;

  msg.pattern = request.mode;
  msg.value = request.value;
  msg.flags = 0;
  msg.flags2 = 0;

  arnl.monitor->setWheelLightDefaultMode(false);
  arnl.robot->comDataN(ArCommands::WHEEL_LIGHT, (const char*)&msg, 4);

  return true;
}

void RobotMonitor::robotMonitorTask()
{

  // a way for user to re-enable motors if disabled -- show a popup dialog in
  // MobileEyes.
  if(motorsDisabledPopupID == 0 && robot && !robot->areMotorsEnabled() && robot->isConnected())
  {
    motorsDisabledPopupID = popupServer->createPopup(&motorsDisabledPopupInfo, &handleMotorsDisabledPopupResponseCB);
  }

  // Set LX wheel light pattern based on robot activity. You could add more
  // conditions/light patterns here if you want.
  if(robot->isEStopPressed())
    robot->comDataN(ArCommands::WHEEL_LIGHT, "\x02\0\0\0", 4); // pattern #2, flash red
  else if(!robot->areMotorsEnabled())
    robot->comDataN(ArCommands::WHEEL_LIGHT, "\x03\0\0\0", 4); // pattern #3, flash yellow

  else if (wheelLightDefault) {
    if(fabs(robot->getVel()) < 5)
      robot->comDataN(ArCommands::WHEEL_LIGHT, "\x0A\0\0\0", 4);  // pattern #10, slow blue flash
    else
      robot->comDataN(ArCommands::WHEEL_LIGHT, "\x09\0\0\0", 4);  // pattern 9, blue sweep.
  }

}


void RobotMonitor::setWheelLightDefaultMode(bool default_on)
{
  wheelLightDefault = default_on;
}