p2os.h
Go to the documentation of this file.
00001 /*
00002  *  P2OS for ROS
00003  *  Copyright (C) 2009
00004  *     David Feil-Seifer, Brian Gerkey, Kasper Stoy,
00005  *      Richard Vaughan, & Andrew Howard
00006  *
00007  *
00008  *  This program is free software; you can redistribute it and/or modify
00009  *  it under the terms of the GNU General Public License as published by
00010  *  the Free Software Foundation; either version 2 of the License, or
00011  *  (at your option) any later version.
00012  *
00013  *  This program is distributed in the hope that it will be useful,
00014  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016  *  GNU General Public License for more details.
00017  *
00018  *  You should have received a copy of the GNU General Public License
00019  *  along with this program; if not, write to the Free Software
00020  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00021  *
00022  */
00023 
00024 #ifndef _P2OSDEVICE_H
00025 #define _P2OSDEVICE_H
00026 
00027 #include <pthread.h>
00028 #include <sys/time.h>
00029 #include <iostream>
00030 #include <string.h>
00031 
00032 #include <p2os_driver/packet.h>
00033 #include <p2os_driver/robot_params.h>
00034 
00035 #include <ros/ros.h>
00036 #include <nav_msgs/Odometry.h>
00037 #include <geometry_msgs/Twist.h>
00038 #include <tf/transform_broadcaster.h>
00039 #include <p2os_msgs/BatteryState.h>
00040 #include <p2os_msgs/MotorState.h>
00041 #include <p2os_msgs/GripperState.h>
00042 #include <p2os_msgs/SonarArray.h>
00043 #include <p2os_msgs/DIO.h>
00044 #include <p2os_msgs/AIO.h>
00045 
00046 #include <diagnostic_updater/publisher.h>
00047 #include <diagnostic_updater/diagnostic_updater.h>
00048 
00054 typedef struct ros_p2os_data
00055 {
00057     nav_msgs::Odometry  position;
00059     p2os_msgs::BatteryState batt;
00061     p2os_msgs::MotorState motors;
00063     p2os_msgs::GripperState gripper;
00065     p2os_msgs::SonarArray sonar;
00067     p2os_msgs::DIO dio;
00069     p2os_msgs::AIO aio;
00071     geometry_msgs::TransformStamped odom_trans;
00072 } ros_p2os_data_t;
00073 
00074 // this is here because we need the above typedef's before including it.
00075 #include "sip.h"
00076 #include "kinecalc.h"
00077 
00078 #include "p2os_ptz.h"
00079 
00080 class SIP;
00081 
00082 // Forward declaration of the KineCalc_Base class declared in kinecalc_base.h
00083 //class KineCalc;
00084 
00085 
00086 class P2OSNode
00087 {
00093   public:
00094     P2OSNode( ros::NodeHandle n );
00095     virtual ~P2OSNode();
00096 
00097   public:
00099     int Setup();
00101     int Shutdown();
00102 
00103     int SendReceive(P2OSPacket* pkt, bool publish_data = true );
00104 
00105     void updateDiagnostics();
00106 
00107     void ResetRawPositions();
00108     void ToggleSonarPower(unsigned char val);
00109     void ToggleMotorPower(unsigned char val);
00110     void StandardSIPPutData(ros::Time ts);
00111 
00112     inline double TicksToDegrees (int joint, unsigned char ticks);
00113     inline unsigned char DegreesToTicks (int joint, double degrees);
00114     inline double TicksToRadians (int joint, unsigned char ticks);
00115     inline unsigned char RadiansToTicks (int joint, double rads);
00116     inline double RadsPerSectoSecsPerTick (int joint, double speed);
00117     inline double SecsPerTicktoRadsPerSec (int joint, double secs);
00118 
00119     void SendPulse (void);
00120     //void spin();
00121     void check_and_set_vel();
00122     void cmdvel_cb( const geometry_msgs::TwistConstPtr &);
00123 
00124     void check_and_set_motor_state();
00125     void cmdmotor_state( const p2os_msgs::MotorStateConstPtr &);
00126 
00127     void check_and_set_gripper_state();
00128     void gripperCallback(const p2os_msgs::GripperStateConstPtr &msg);
00129     double get_pulse() {return pulse;}
00130 
00131                 // diagnostic messages
00132                 void check_voltage( diagnostic_updater::DiagnosticStatusWrapper &stat );
00133                 void check_stall( diagnostic_updater::DiagnosticStatusWrapper &stat );
00134 
00135   protected:
00137     ros::NodeHandle n;
00139     ros::NodeHandle nh_private;
00140  
00141     diagnostic_updater::Updater diagnostic_;
00142 
00143     diagnostic_updater::DiagnosedPublisher<p2os_msgs::BatteryState> batt_pub_;
00144     ros::Publisher mstate_pub_, grip_state_pub_,
00145       ptz_state_pub_, sonar_pub_, aio_pub_, dio_pub_;
00146     ros::Publisher pose_pub_;
00147     ros::Subscriber cmdvel_sub_, cmdmstate_sub_, gripper_sub_, ptz_cmd_sub_;
00148 
00149     tf::TransformBroadcaster odom_broadcaster;
00150     ros::Time veltime;
00151 
00152     SIP* sippacket;
00153     std::string psos_serial_port;
00154     std::string psos_tcp_host;
00155     std::string odom_frame_id;
00156     std::string base_link_frame_id;
00157     int         psos_fd;
00158     bool        psos_use_tcp;
00159     int         psos_tcp_port;
00160     bool        vel_dirty, motor_dirty;
00161     bool        gripper_dirty_;
00162     int         param_idx;
00163     // PID settings
00164     int rot_kp, rot_kv, rot_ki, trans_kp, trans_kv, trans_ki;
00165 
00167     int bumpstall; // should we change the bumper-stall behavior?
00169     int joystick;
00171     int direct_wheel_vel_control;
00172     int radio_modemp;
00173 
00175     int motor_max_speed;
00177     int motor_max_turnspeed;
00179     short motor_max_trans_accel;
00181     short motor_max_trans_decel;
00183     short motor_max_rot_accel;
00185     short motor_max_rot_decel;
00187     double pulse;
00188     double desired_freq;
00190     double lastPulseTime;
00192     bool use_sonar_;
00193 
00194     P2OSPtz ptz_;
00195 
00196   public:
00198     geometry_msgs::Twist cmdvel_;
00200     p2os_msgs::MotorState  cmdmotor_state_;
00202     p2os_msgs::GripperState gripper_state_;
00204     ros_p2os_data_t p2os_data;
00205 };
00206 
00207 #endif


p2os_driver
Author(s): Hunter L. Allen , David Feil-Seifer , Aris Synodinos , Brian Gerkey, Kasper Stoy, Richard Vaughan, Andrew Howard, Tucker Hermans, ActivMedia Robotics LLC, MobileRobots Inc.
autogenerated on Sat Aug 5 2017 02:20:17