Rmp440LE.h

Go to the documentation of this file.

/*
  COPYRIGHT (c) 2014 SEGWAY Inc.

  Software License Agreement:

  The software supplied herewith by Segway Inc. (the "Company") for its 
  RMP Robotic Platforms is intended and supplied to you, the Company's 
  customer, for use solely and exclusively with Segway products. The 
  software is owned by the Company and/or its supplier, and is protected 
  under applicable copyright laws.  All rights are reserved. Any use in 
  violation of the foregoing restrictions may subject the user to criminal
  sanctions under applicable laws, as well as to civil liability for the 
  breach of the terms and conditions of this license. The Company may 
  immediately terminate this Agreement upon your use of the software with 
  any products that are not Segway products.

  You shall indemnify, defend and hold the Company harmless from any claims, 
  demands, liabilities or expenses, including reasonable attorneys fees, incurred 
  by the Company as a result of any claim or proceeding against the Company 
  arising out of or based upon: 

  (i) The combination, operation or use of the software by you with any hardware, 
      products, programs or data not supplied or approved in writing by the Company, 
      if such claim or proceeding would have been avoided but for such combination, 
      operation or use.

  (ii) The modification of the software by or on behalf of you.

  (iii) Your use of the software.

  THIS SOFTWARE IS PROVIDED IN AN "AS IS" CONDITION. NO WARRANTIES,
  WHETHER EXPRESS, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED
  TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
  PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. THE COMPANY SHALL NOT,
  IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL OR
  CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
*/

#ifndef RMP_440LE_H
#define RMP_440LE_H 

#include <ros/ros.h>
#include <geometry_msgs/TwistStamped.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <rmp_msgs/BoolStamped.h>
#include <rmp_msgs/AudioCommand.h>
#include <rmp_msgs/FaultStatus.h>

#include <Rmp440LEInterface.h>

struct WheelsDisplacement
{
  WheelsDisplacement()
    : m_Linear(0.0)
    , m_LeftFront(0.0)
    , m_LeftRear(0.0)
    , m_RightFront(0.0)
    , m_RightRear(0.0)
  {}

  double m_Linear;
  
  double m_LeftFront;
  
  double m_LeftRear;
  
  double m_RightFront;
  
  double m_RightRear;
};

class Rmp440LE
{
public:
  Rmp440LE();
  
  ~Rmp440LE();
  
  void Initialize();
  
private:
  void InitializeMessages();

  void ProcessVelocityCommand(const geometry_msgs::TwistStamped::ConstPtr& rpVelocityCommand);

  void ProcessDeadman(const rmp_msgs::BoolStamped::ConstPtr& rpDeadmanMsg);

  void ProcessAudioCommand(const rmp_msgs::AudioCommand::ConstPtr& rpAudioCommand);

  void UpdateStatus();

  void UpdateImu();

  void UpdateOdometry();

  void UpdateBattery();

  void UpdateMotorStatus();

  void UpdateFaultStatus();

  bool IsDeadmanValid();

  segway::Rmp440LEInterface m_Rmp440LEInterface;

  ros::NodeHandle m_NodeHandle;

  ros::Publisher m_OdometryPublisher;

  ros::Publisher m_JointStatesPublisher;

  ros::Publisher m_InertialPublisher;

  ros::Publisher m_PsePublisher;

  ros::Publisher m_MotorStatusPublisher;

  ros::Publisher m_BatteryPublisher;

  ros::Publisher m_FaultStatusPublisher;

  ros::Subscriber m_VelocityCommandSubscriber;

  ros::Subscriber m_DeadmanSubscriber;

  ros::Subscriber m_AudioCommandSubscriber;

  nav_msgs::Odometry m_OdometryMsg;

  sensor_msgs::JointState m_JointStateMsg;

  sensor_msgs::Imu m_InertialMsg;

  sensor_msgs::Imu m_PseMsg;

  rmp_msgs::BoolStamped m_DeadmanMsg;

  WheelsDisplacement m_WheelsDisplacement;

  bool m_OdometryInitialized;
};

#endif // RMP_440LE_H