RemKhepServerROSPluginFuerte.h
Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00031
00032 #ifndef __REM_KHEP_SRV_ROS_PLG_H_
00033 #define __REM_KHEP_SRV_ROS_PLG_H_
00034
00035
00036 #include <CommonOptions.h>
00037 #include <Option.h>
00038 #include "RemKhepServerROS.h"
00039
00040
00041
00042
00043 #include <geometry_msgs/Twist.h>
00044 #include <nav_msgs/Odometry.h>
00045 #include <ros/ros.h>
00046 #include <tf/transform_broadcaster.h>
00047
00048
00049 #include "gazebo.hh"
00050 #include <sdf/interface/Param.hh>
00051 #include <physics/physics.h>
00052 #include <common/CommonTypes.hh>
00053 #include <common/common.h>
00054
00055
00056 #include <ros/callback_queue.h>
00057 #include <ros/advertise_options.h>
00058
00059
00060 #include <boost/thread.hpp>
00061
00062
00063 #define SAMPLE_TIME_USEC 30000
00064 #define WAIT_TIME_USEC 1000
00065
00066 #define KHS_COMM_DEBUG 0
00067 #define KHS_TIME_DEBUG 0
00068 #define KHS_EXECUTE_DEBUG 0
00069
00070 namespace gazebo{
00071
00072 class remKhepSrvROSPlugin : public ModelPlugin{
00073 public:
00075 remKhepSrvROSPlugin();
00076
00078 virtual ~remKhepSrvROSPlugin();
00079
00080 protected:
00082 virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
00083
00085 void SaveChild(std::string &prefix, std::ostream &stream);
00086 void InitChild();
00087 void ResetChild();
00088 void UpdateChild();
00089 void FiniChild();
00090
00091 private:
00092
00093 void write_position_data();
00094 void publish_odometry();
00095
00096
00097 physics::ModelPtr parent_;
00098 physics::WorldPtr world;
00099 event::ConnectionPtr updateConnection;
00100
00101
00102 Decimal wheelSep;
00103
00104 Decimal wheelDiam;
00105 Decimal torque;
00106
00107 Decimal update_rate;
00108 Decimal update_period;
00109 common::Time last_update_time;
00110
00111 bool enableMotors;
00112 int debugLevel;
00113 int urg04lxActive;
00114 string intMethodOdo;
00115 string laserTopicName;
00116 string topicName;
00117 string nodeName;
00118 string commType;
00119 Decimal encodRes;
00120 Decimal wheelSpeed[2];
00121 string leftJointName;
00122 string rightJointName;
00123 string robotNamespace;
00124 string topicNameOdom;
00125
00126
00127
00128 KheperaServerROSPars* _khPars;
00129
00130
00131
00132
00133
00134 common::Time _execTimer;
00135
00136
00137 KheperaServerROS* _remKhepSrvROS;
00138
00139
00140 bool _firstRun;
00141
00142 bool _connected;
00143
00144 int _actCmd;
00145
00146 GAZEBO_JOINT_PTR joints[2];
00147 physics::PhysicsEngine *physicsEngine;
00148
00149
00150
00151 Decimal d1;
00152 Decimal d2;
00153 Decimal dr;
00154 Decimal da;
00155
00156 Time stepTime;
00157
00158
00159
00160 ros::Publisher pub_;
00161 ros::Subscriber sub_;
00162
00163 std::string tf_prefix_;
00164
00165 boost::mutex lock;
00166
00167
00168 ros::CallbackQueue queue_;
00169 boost::thread* callback_queue_thread_;
00170 tf::TransformBroadcaster br;
00171
00172 void QueueThread();
00173
00174
00175 void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg);
00176
00177 Decimal _xVel;
00178 Decimal _omega;
00179 bool alive_;
00180 };
00181
00182 }
00183
00184 #endif
00185