p2os.h
Go to the documentation of this file.
1 /*
2  * P2OS for ROS
3  * Copyright (C) 2009
4  * David Feil-Seifer, Brian Gerkey, Kasper Stoy,
5  * Richard Vaughan, & Andrew Howard
6  *
7  *
8  * This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program; if not, write to the Free Software
20  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
21  *
22  */
23 
24 #ifndef _P2OSDEVICE_H
25 #define _P2OSDEVICE_H
26 
27 #include <pthread.h>
28 #include <sys/time.h>
29 #include <iostream>
30 #include <string.h>
31 
32 #include <p2os_driver/packet.h>
34 
35 #include <ros/ros.h>
36 #include <nav_msgs/Odometry.h>
37 #include <geometry_msgs/Twist.h>
38 #include <tf/transform_broadcaster.h>
39 #include <p2os_msgs/BatteryState.h>
40 #include <p2os_msgs/MotorState.h>
41 #include <p2os_msgs/GripperState.h>
42 #include <p2os_msgs/SonarArray.h>
43 #include <p2os_msgs/DIO.h>
44 #include <p2os_msgs/AIO.h>
45 
46 #include <diagnostic_updater/publisher.h>
47 #include <diagnostic_updater/diagnostic_updater.h>
48 
54 typedef struct ros_p2os_data
55 {
57  nav_msgs::Odometry position;
59  p2os_msgs::BatteryState batt;
61  p2os_msgs::MotorState motors;
63  p2os_msgs::GripperState gripper;
65  p2os_msgs::SonarArray sonar;
67  p2os_msgs::DIO dio;
69  p2os_msgs::AIO aio;
71  geometry_msgs::TransformStamped odom_trans;
73 
74 // this is here because we need the above typedef's before including it.
75 #include "sip.h"
76 #include "kinecalc.h"
77 
78 #include "p2os_ptz.h"
79 
80 class SIP;
81 
82 // Forward declaration of the KineCalc_Base class declared in kinecalc_base.h
83 //class KineCalc;
84 
85 
86 class P2OSNode
87 {
93  public:
94  P2OSNode( ros::NodeHandle n );
95  virtual ~P2OSNode();
96 
97  public:
99  int Setup();
101  int Shutdown();
102 
103  int SendReceive(P2OSPacket* pkt, bool publish_data = true );
104 
105  void updateDiagnostics();
106 
107  void ResetRawPositions();
108  void ToggleSonarPower(unsigned char val);
109  void ToggleMotorPower(unsigned char val);
110  void StandardSIPPutData(ros::Time ts);
111 
112  inline double TicksToDegrees (int joint, unsigned char ticks);
113  inline unsigned char DegreesToTicks (int joint, double degrees);
114  inline double TicksToRadians (int joint, unsigned char ticks);
115  inline unsigned char RadiansToTicks (int joint, double rads);
116  inline double RadsPerSectoSecsPerTick (int joint, double speed);
117  inline double SecsPerTicktoRadsPerSec (int joint, double secs);
118 
119  void SendPulse (void);
120  //void spin();
121  void check_and_set_vel();
122  void cmdvel_cb( const geometry_msgs::TwistConstPtr &);
123 
125  void cmdmotor_state( const p2os_msgs::MotorStateConstPtr &);
126 
128  void gripperCallback(const p2os_msgs::GripperStateConstPtr &msg);
129  double get_pulse() {return pulse;}
130 
131  // diagnostic messages
132  void check_voltage( diagnostic_updater::DiagnosticStatusWrapper &stat );
133  void check_stall( diagnostic_updater::DiagnosticStatusWrapper &stat );
134 
135  protected:
137  ros::NodeHandle n;
139  ros::NodeHandle nh_private;
140 
141  diagnostic_updater::Updater diagnostic_;
142 
143  diagnostic_updater::DiagnosedPublisher<p2os_msgs::BatteryState> batt_pub_;
144  ros::Publisher mstate_pub_, grip_state_pub_,
146  ros::Publisher pose_pub_;
148 
149  tf::TransformBroadcaster odom_broadcaster;
150  ros::Time veltime;
151 
153  std::string psos_serial_port;
154  std::string psos_tcp_host;
155  int psos_fd;
161  // PID settings
163 
165  int bumpstall; // should we change the bumper-stall behavior?
167  int joystick;
171 
185  double pulse;
186  double desired_freq;
191 
193 
194  public:
196  geometry_msgs::Twist cmdvel_;
198  p2os_msgs::MotorState cmdmotor_state_;
200  p2os_msgs::GripperState gripper_state_;
203 };
204 
205 #endif
std::string psos_tcp_host
Definition: p2os.h:154
double TicksToDegrees(int joint, unsigned char ticks)
Convert ticks to degrees.
Definition: p2os.cc:876
short motor_max_trans_decel
Minimum translational acceleration in Meters per second per second.
Definition: p2os.h:179
diagnostic_updater::Updater diagnostic_
Definition: p2os.h:141
int rot_kv
Definition: p2os.h:162
virtual ~P2OSNode()
Definition: p2os.cc:130
ros::Publisher aio_pub_
Definition: p2os.h:144
Container struct.
Definition: p2os.h:54
int motor_max_speed
Maximum motor speed in Meters per second.
Definition: p2os.h:173
int rot_kp
Definition: p2os.h:162
struct ros_p2os_data ros_p2os_data_t
Container struct.
bool motor_dirty
Definition: p2os.h:158
int trans_kv
Definition: p2os.h:162
void ToggleMotorPower(unsigned char val)
Toggle motors on/off.
Definition: p2os.cc:856
ros::Publisher mstate_pub_
Definition: p2os.h:144
int SendReceive(P2OSPacket *pkt, bool publish_data=true)
Definition: p2os.cc:724
double get_pulse()
Definition: p2os.h:129
p2os_msgs::MotorState cmdmotor_state_
Motor state publisher.
Definition: p2os.h:198
p2os_msgs::SonarArray sonar
Container for sonar data.
Definition: p2os.h:65
double RadsPerSectoSecsPerTick(int joint, double speed)
Convert radians per second to radians per encoder tick.
Definition: p2os.cc:929
int psos_tcp_port
Definition: p2os.h:157
ros::Publisher dio_pub_
Definition: p2os.h:144
int rot_ki
Definition: p2os.h:162
bool use_sonar_
Use the sonar array?
Definition: p2os.h:190
SIP * sippacket
Definition: p2os.h:152
unsigned char RadiansToTicks(int joint, double rads)
Convert radians to ticks.
Definition: p2os.cc:922
void updateDiagnostics()
Definition: p2os.cc:784
p2os_msgs::AIO aio
Analog In/Out.
Definition: p2os.h:69
ros::NodeHandle n
Node Handler used for publication of data.
Definition: p2os.h:137
void check_voltage(diagnostic_updater::DiagnosticStatusWrapper &stat)
Definition: p2os.cc:789
std::string psos_serial_port
Definition: p2os.h:153
int joystick
Use Joystick?
Definition: p2os.h:167
void gripperCallback(const p2os_msgs::GripperStateConstPtr &msg)
Definition: p2os.cc:267
p2os_msgs::GripperState gripper
Provides the state of the gripper.
Definition: p2os.h:63
geometry_msgs::TransformStamped odom_trans
Transformed odometry frame.
Definition: p2os.h:71
ros::Publisher sonar_pub_
Definition: p2os.h:144
void check_and_set_gripper_state()
Definition: p2os.cc:159
tf::TransformBroadcaster odom_broadcaster
Definition: p2os.h:149
void SendPulse(void)
Definition: p2os.cc:954
diagnostic_updater::DiagnosedPublisher< p2os_msgs::BatteryState > batt_pub_
Definition: p2os.h:143
int bumpstall
Stall I hit a wall?
Definition: p2os.h:165
short motor_max_rot_accel
Maximum rotational acceleration in radians per second per second.
Definition: p2os.h:181
double pulse
Pulse time.
Definition: p2os.h:185
p2os_msgs::GripperState gripper_state_
Gripper state publisher.
Definition: p2os.h:200
int trans_ki
Definition: p2os.h:162
ros::Publisher grip_state_pub_
Definition: p2os.h:144
double desired_freq
Definition: p2os.h:186
int Setup()
Setup the robot for use. Communicates with the robot directly.
Definition: p2os.cc:273
bool vel_dirty
Definition: p2os.h:158
bool psos_use_tcp
Definition: p2os.h:156
ros::Time veltime
Definition: p2os.h:150
P2OSNode(ros::NodeHandle n)
P2OS robot driver node.
Definition: p2os.cc:32
ros::Subscriber ptz_cmd_sub_
Definition: p2os.h:147
P2OSPtz ptz_
Definition: p2os.h:192
void cmdmotor_state(const p2os_msgs::MotorStateConstPtr &)
Definition: p2os.cc:134
int trans_kp
Definition: p2os.h:162
ros::Subscriber cmdvel_sub_
Definition: p2os.h:147
void ResetRawPositions()
Definition: p2os.cc:818
p2os_msgs::BatteryState batt
Provides the battery voltage.
Definition: p2os.h:59
int motor_max_turnspeed
Maximum turn speed in radians per second.
Definition: p2os.h:175
int Shutdown()
Prepare for shutdown.
Definition: p2os.cc:657
double lastPulseTime
Last time the node received or sent a pulse.
Definition: p2os.h:188
Definition: sip.h:54
int psos_fd
Definition: p2os.h:155
ros::Subscriber gripper_sub_
Definition: p2os.h:147
bool gripper_dirty_
Definition: p2os.h:159
ros::Subscriber cmdmstate_sub_
Definition: p2os.h:147
void StandardSIPPutData(ros::Time ts)
Definition: p2os.cc:693
void ToggleSonarPower(unsigned char val)
Definition: p2os.cc:838
int param_idx
Definition: p2os.h:160
ros::NodeHandle nh_private
Node Handler used for private data publication.
Definition: p2os.h:139
short motor_max_trans_accel
Maximum translational acceleration in Meters per second per second.
Definition: p2os.h:177
double TicksToRadians(int joint, unsigned char ticks)
Convert ticks to radians.
Definition: p2os.cc:915
void check_and_set_vel()
Definition: p2os.cc:210
short motor_max_rot_decel
Minimum rotational acceleration in Meters per second per second.
Definition: p2os.h:183
ros_p2os_data_t p2os_data
sensor data container
Definition: p2os.h:202
geometry_msgs::Twist cmdvel_
Command Velocity subscriber.
Definition: p2os.h:196
Definition: p2os.h:86
p2os_msgs::MotorState motors
Provides the state of the motors (enabled or disabled)
Definition: p2os.h:61
int radio_modemp
Definition: p2os.h:170
double SecsPerTicktoRadsPerSec(int joint, double secs)
Convert Seconds per encoder tick to radians per second.
Definition: p2os.cc:944
unsigned char DegreesToTicks(int joint, double degrees)
convert degrees to ticks
Definition: p2os.cc:893
void check_and_set_motor_state()
Definition: p2os.cc:140
void cmdvel_cb(const geometry_msgs::TwistConstPtr &)
Definition: p2os.cc:187
ros::Publisher ptz_state_pub_
Definition: p2os.h:144
nav_msgs::Odometry position
Provides the position of the robot.
Definition: p2os.h:57
int direct_wheel_vel_control
Control wheel velocities individually?
Definition: p2os.h:169
void check_stall(diagnostic_updater::DiagnosticStatusWrapper &stat)
Definition: p2os.cc:806
p2os_msgs::DIO dio
Digital In/Out.
Definition: p2os.h:67
ros::Publisher pose_pub_
Definition: p2os.h:146


p2os_driver
Author(s): Hunter Allen , David Feil-Seifer , Aris Synodinos , Brian Gerkey, Kasper Stoy, Richard Vaughan, Andrew Howard, Tucker Hermans, ActivMedia Robotics LLC, MobileRobots Inc.
autogenerated on Wed Jun 25 2014 09:37:15