p2os.hpp

Go to the documentation of this file.

/*
 *  P2OS for ROS
 *  Copyright (C) 2009  David Feil-Seifer, Brian Gerkey, Kasper Stoy,
 *      Richard Vaughan, & Andrew Howard
 *  Copyright (C) 2018  Hunter L. Allen
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
#ifndef P2OS_DRIVER__P2OS_HPP_
#define P2OS_DRIVER__P2OS_HPP_
#include <pthread.h>
#include <sys/time.h>

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_broadcaster.h>
#include <p2os_msgs/BatteryState.h>
#include <p2os_msgs/MotorState.h>
#include <p2os_msgs/GripperState.h>
#include <p2os_msgs/SonarArray.h>
#include <p2os_msgs/DIO.h>
#include <p2os_msgs/AIO.h>

#include <diagnostic_updater/publisher.h>
#include <diagnostic_updater/diagnostic_updater.h>

#include <p2os_driver/packet.hpp>
#include <p2os_driver/robot_params.hpp>

#include <iostream>
#include <cstring>
#include <string>

typedef struct ros_p2os_data
{
  nav_msgs::Odometry position;
  p2os_msgs::BatteryState batt;
  p2os_msgs::MotorState motors;
  p2os_msgs::GripperState gripper;
  p2os_msgs::SonarArray sonar;
  p2os_msgs::DIO dio;
  p2os_msgs::AIO aio;
  geometry_msgs::TransformStamped odom_trans;
} ros_p2os_data_t;

// this is here because we need the above typedef's before including it.
#include "sip.hpp"
#include "kinecalc.hpp"

#include "p2os_ptz.hpp"

class SIP;

// Forward declaration of the KineCalc_Base class declared in kinecalc_base.h

class P2OSNode
{
public:
  explicit P2OSNode(ros::NodeHandle n);
  virtual ~P2OSNode();

public:
  int Setup();
  int Shutdown();

  int SendReceive(P2OSPacket * pkt, bool publish_data = true);

  void updateDiagnostics();

  void ResetRawPositions();
  void ToggleSonarPower(unsigned char val);
  void ToggleMotorPower(unsigned char val);
  void StandardSIPPutData(ros::Time ts);

  inline double TicksToDegrees(int joint, unsigned char ticks);
  inline unsigned char DegreesToTicks(int joint, double degrees);
  inline double TicksToRadians(int joint, unsigned char ticks);
  inline unsigned char RadiansToTicks(int joint, double rads);
  inline double RadsPerSectoSecsPerTick(int joint, double speed);
  inline double SecsPerTicktoRadsPerSec(int joint, double secs);

  void SendPulse(void);

  void check_and_set_vel();
  void cmdvel_cb(const geometry_msgs::TwistConstPtr &);

  void check_and_set_motor_state();
  void cmdmotor_state(const p2os_msgs::MotorStateConstPtr &);

  void check_and_set_gripper_state();
  void gripperCallback(const p2os_msgs::GripperStateConstPtr & msg);
  double get_pulse() {return pulse;}

  // diagnostic messages
  void check_voltage(diagnostic_updater::DiagnosticStatusWrapper & stat);
  void check_stall(diagnostic_updater::DiagnosticStatusWrapper & stat);

protected:
  ros::NodeHandle n;
  ros::NodeHandle nh_private;

  diagnostic_updater::Updater diagnostic_;

  diagnostic_updater::DiagnosedPublisher<p2os_msgs::BatteryState> batt_pub_;
  ros::Publisher mstate_pub_, grip_state_pub_,
    ptz_state_pub_, sonar_pub_, aio_pub_, dio_pub_;
  ros::Publisher pose_pub_;
  ros::Subscriber cmdvel_sub_, cmdmstate_sub_, gripper_sub_, ptz_cmd_sub_;

  tf::TransformBroadcaster odom_broadcaster;
  ros::Time veltime;

  SIP * sippacket;
  std::string psos_serial_port;
  std::string psos_tcp_host;
  std::string odom_frame_id;
  std::string base_link_frame_id;
  int psos_fd;
  bool psos_use_tcp;
  int psos_tcp_port;
  bool vel_dirty, motor_dirty;
  bool gripper_dirty_ = false;
  int param_idx;
  // PID settings
  int rot_kp, rot_kv, rot_ki, trans_kp, trans_kv, trans_ki;

  int bumpstall;   // should we change the bumper-stall behavior?
  int joystick;
  int direct_wheel_vel_control;
  int radio_modemp;

  int motor_max_speed;
  int motor_max_turnspeed;
  int16_t motor_max_trans_accel;
  int16_t motor_max_trans_decel;
  int16_t motor_max_rot_accel;
  int16_t motor_max_rot_decel;
  double pulse;
  double desired_freq;
  double lastPulseTime;
  bool use_sonar_;

  P2OSPtz ptz_;

public:
  geometry_msgs::Twist cmdvel_;
  p2os_msgs::MotorState cmdmotor_state_;
  p2os_msgs::GripperState gripper_state_;
  ros_p2os_data_t p2os_data;
};

#endif  // P2OS_DRIVER__P2OS_HPP_