$search

remote_power_manager.cpp

Go to the documentation of this file.

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Robert Bosch LLC.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Robert Bosch nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *********************************************************************/

//\Author Philip Roan, Bosch LLC

#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <ros/ros.h>
#include "remote_power_manager.h"


const char WILL = 251;
const char WONT = 252;
const char DO   = 253;
const char DONT = 254;
const char IAC  = 255; // control sequence
const char ECHO = 1;   // Echo commands
const char SGA  = 3;   // Suppress Go-Ahead

const int SOCKET_BUFFER = 100;

RemotePowerManager::RemotePowerManager(const ros::NodeHandle& nh) :
  host_name_("butterbur"),
  host_port_(23),
  number_of_power_ports_(2),
  socket_(0)
{
  // Getting parameters from .launch file
  nh.getParam("/remote_power_manager_node/host_name", host_name_);
  nh.getParam("/remote_power_manager_node/host_port", host_port_);
  nh.getParam("/remote_power_manager_node/number_of_power_ports", number_of_power_ports_);

  //ROS_INFO("Retrieved parameters: %s %d %d", host_name_.c_str(), host_port_, number_of_power_ports_);

  // Set defaults if unavailable?
}

int RemotePowerManager::parseCommands(char* raw_message, int length, char* text_message)
{
  int i = 0;
  int j = 0;
  char response[3];

  //ROS_INFO("Entering parse_commands");
  for( i = 0; i < length; i++ )
  {
    //ROS_INFO("i %d byte %u", i, raw_message[i] & 0xFF);
    if( raw_message[i] == IAC )
    {
      switch( raw_message[++i] )
      {
      case IAC:
        // escaped sequence, so put 255 into message
        text_message[j++] = 255;
        break;
      case DO:
      case DONT:
      case WILL:
      case WONT:
        //ROS_INFO("-i %d byte %u", i, raw_message[i] & 0xFF);
        // reply to all commands with "WONT" except SGA request
        response[0] = IAC;
        switch( raw_message[++i] )
        {
        //case (char)1: //ECHO option
        case SGA:
          if( raw_message[i-1] == DO )
          {
            response[1] = WILL;
          }
          else
          {
            response[1] = DO;
          }
          break;          
        default:
          if( raw_message[i-1] == DO )
          {
            response[1] = WONT;
          }
          else
          {
            response[1] = DONT;
          }       
          break;
        }
        response[2] = raw_message[i];
        //ROS_DEBUG("received: %u %u %u, responding: %u %u %u",
        //       raw_message[i-2] & 0xFF,
        //       raw_message[i-1] & 0xFF,
        //       raw_message[i] & 0xFF,
        //       response[0] & 0xFF,
        //       response[1] & 0xFF,
        //       response[2] & 0xFF);
        send( socket_, response, 3, 0);
        break;
      default:
        // unsupported command, ignore option
        ROS_WARN("Unsupported telnet command received: %u", raw_message[i] & 0xFF);
        i++;
        break;
      }
    }
    else
    {
      text_message[j++] = raw_message[i];
    }
  }

  return j;
}


bool RemotePowerManager::openConnection(void)
{
  struct hostent *host;
  struct sockaddr_in server_addr;
  char recv_data[SOCKET_BUFFER];
  char send_data[SOCKET_BUFFER];
  char message[SOCKET_BUFFER];
  int bytes_received;

  host = gethostbyname( host_name_.c_str() );

  // Open socket
  if( (socket_ = socket(AF_INET, SOCK_STREAM, 0)) == -1 )
  {
    ROS_ERROR("Unable to open socket.");
    return false;
  }

  server_addr.sin_family = AF_INET;     
  server_addr.sin_port = htons( host_port_ );   
  server_addr.sin_addr = *((struct in_addr *)host->h_addr);
  bzero(&(server_addr.sin_zero),8); 

  // Connect to server
  if( connect(socket_, (struct sockaddr *)&server_addr, sizeof(struct sockaddr)) == -1 ) 
  {
    ROS_ERROR("Unable to connect to server.");
    return false;
  }

  // Read intial response and parse for command signal IAC
  bytes_received = recv( socket_, recv_data, SOCKET_BUFFER, 0 );
  if( bytes_received < SOCKET_BUFFER )
  {
    recv_data[bytes_received] = '\0';
    parseCommands(recv_data, bytes_received+1, message);
  }
  else
  {
    recv_data[bytes_received-1] = '\0';
    parseCommands(recv_data, bytes_received, message);
  }
  ROS_DEBUG("Remote Power System login message:%s", message);

  //Echo - the magic seems to be in sending two packets
  memset(send_data, '\n', 28);
  send( socket_, send_data, 28, 0 );
  //for(int k = 0; k < strlen(send_data); k++)
  //{
  //  ROS_DEBUG("magic echo[%d]: %02x", k, send_data[k]);
  //}

  return true;
}

bool RemotePowerManager::closeConnection(void)
{
  char send_data[SOCKET_BUFFER];
  char recv_data[SOCKET_BUFFER];
  int bytes_received;

  // Client is closing connection
  
  sprintf( send_data, "$A2\r");
  
  send( socket_, send_data, strlen(send_data), 0);
  bytes_received = recv( socket_, recv_data, SOCKET_BUFFER, 0 );
  if( bytes_received < SOCKET_BUFFER )
  {
    recv_data[bytes_received] = '\0';
  }
  else
  {
    recv_data[bytes_received-1] = '\0';
  }
  ROS_DEBUG("Closing connection to Remote Power Manager: %s", recv_data);
  
  close(socket_);

  return true;
}

bool RemotePowerManager::sendCommand(const char command[], char reply[])
{
  int bytes_received;

  // Send command
  send(socket_, command, strlen(command), 0);   

  // Copy received data from the socket
  bytes_received = recv( socket_, reply, SOCKET_BUFFER, 0 );
  if( bytes_received < SOCKET_BUFFER )
  {
    reply[bytes_received] = '\0';
  }
  else
  {
    reply[bytes_received-1] = '\0';
  }
  
  ROS_DEBUG("Received %d bytes from Remote Power Manager: %s", bytes_received, reply);
  if( strstr( reply, "$A0" ) != NULL )
  {
    return true;
  }
  else if( strstr( reply, "$AF" ) != NULL )
  {
    return false;
  }
  else
  {
    ROS_WARN("Unknown response from remote power manager.");
    return false;
  }
}


bool RemotePowerManager::getPortStatus( remote_power_manager::GetPortStatus::Request  &req,
                                          remote_power_manager::GetPortStatus::Response &res )
{
  char reply[SOCKET_BUFFER];
  char* p_status; 

  if( openConnection() == false )
  {
    return false;
  }

  if( sendCommand("\r$A5\r", reply) )
  {
    res.value.port_status.clear();
    // parse first comma
    p_status = strchr( reply, ',' );
    if( p_status != NULL )
    {
      for( int i = 0; i < number_of_power_ports_; i++ )
      {
        p_status++;
        if( *p_status == '1' )
        {
          res.value.port_status.insert(res.value.port_status.begin(), true);
        }
        else
        {
          res.value.port_status.insert(res.value.port_status.begin(), false);
        }
      }
      closeConnection();
      return true;
    }
    else
    {
      ROS_WARN("Failed to get port status from remote power manager.");
      closeConnection();
      return false;
    }
  }
  else
  {
    ROS_WARN("Failed to get port status from remote power manager.");
    closeConnection();
    return false;
  }
}

bool RemotePowerManager::setAllRelay( remote_power_manager::SetAllRelay::Request  &req,
                                        remote_power_manager::SetAllRelay::Response &res )
{
  char command[SOCKET_BUFFER];
  char reply[SOCKET_BUFFER];

  if( openConnection() == false )
  {
    return false;
  }
 
  sprintf( command, "\r$A7 %d\r", (int)req.value );

  if( sendCommand(command, reply) )
  {
    closeConnection();
    res.return_code = true;
    return true;
  }
  else
  {
    ROS_WARN("Failed to set all relays to \"%d\" on remote power manager.", (int)req.value);
    closeConnection();
    res.return_code = true;
    return false;
  }
}


bool RemotePowerManager::setPortRelay( remote_power_manager::SetPortRelay::Request  &req,
                                         remote_power_manager::SetPortRelay::Response &res )
{
  char command[SOCKET_BUFFER];
  char reply[SOCKET_BUFFER];

  if( openConnection() == false )
  {
    return false;
  }
 
  sprintf( command, "\r$A3 %ld %d\r", req.power_port, (int)req.value );

  if( sendCommand(command, reply) )
  {
    closeConnection();
    res.return_code = true;
    return true;
  }
  else
  {
    ROS_WARN("Failed to set relay %ld to \"%d\" on remote power manager.", req.power_port, (int)req.value);
    closeConnection();
    res.return_code = true;
    return false;
  }
}



bool RemotePowerManager::rebootPort( remote_power_manager::RebootPort::Request  &req,
                                      remote_power_manager::RebootPort::Response &res )
{
  char command[SOCKET_BUFFER];
  char reply[SOCKET_BUFFER];

  if( openConnection() == false )
  {
    return false;
  }
  sprintf( command, "\r$A4 %ld\r", req.power_port );

  if( sendCommand(command, reply) )
  {
    closeConnection();
    res.return_code = true;
    return true;
  }
  else
  {
    ROS_WARN("Failed to reboot relay %ld on remote power manager.", req.power_port);
    closeConnection();
    res.return_code = true;
    return false;
  }
}


int main(int argc, char **argv)
{
  ros::init(argc, argv, "remote_power_manager_node");

  ros::NodeHandle nh;
  RemotePowerManager rmp = RemotePowerManager(nh);


  ros::ServiceServer service_handle_1 = nh.advertiseService("remote_power_manager/get_port_status",
                                                            &RemotePowerManager::getPortStatus, &rmp);
  ros::ServiceServer service_handle_2 = nh.advertiseService("remote_power_manager/set_all_relay",
                                                            &RemotePowerManager::setAllRelay, &rmp);
  ros::ServiceServer service_handle_3 = nh.advertiseService("remote_power_manager/set_port_relay",
                                                            &RemotePowerManager::setPortRelay, &rmp);
  ros::ServiceServer service_handle_4 = nh.advertiseService("remote_power_manager/reboot_port",
                                                            &RemotePowerManager::rebootPort, &rmp);

  ros::spin();

  return 0;
}

---

remote_power_manager
Author(s): Philip Roan, Bosch LLC
autogenerated on Sun Mar 3 12:02:56 2013