00001 00025 #ifndef GAZEBO_ROS_CREATE_H 00026 #define GAZEBO_ROS_CREATE_H 00027 00028 #include "gazebo/physics/physics.hh" 00029 #include "gazebo/physics/PhysicsTypes.hh" 00030 #include "gazebo/sensors/SensorTypes.hh" 00031 #include "gazebo/transport/TransportTypes.hh" 00032 #include "gazebo/common/Time.hh" 00033 #include "gazebo/common/Plugin.hh" 00034 #include "gazebo/common/Events.hh" 00035 00036 #include <nav_msgs/Odometry.h> 00037 #include <grizzly_msgs/Drive.h> 00038 #include <grizzly_msgs/eigen.h> 00039 #include <geometry_msgs/TwistWithCovariance.h> 00040 #include <geometry_msgs/PoseWithCovariance.h> 00041 00042 #include <tf/transform_broadcaster.h> 00043 #include <ros/ros.h> 00044 00045 // Custom Callback Queue 00046 #include <ros/callback_queue.h> 00047 #include <ros/advertise_options.h> 00048 00049 00050 namespace gazebo 00051 { 00052 class GrizzlyPlugin : public ModelPlugin 00053 { 00054 public: 00055 GrizzlyPlugin(); 00056 virtual ~GrizzlyPlugin(); 00057 virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf ); 00058 00059 protected: 00060 virtual void UpdateChild(); 00061 virtual void FiniChild(); 00062 00063 private: 00064 void OnContact(const std::string &name, const physics::Contact &contact); 00065 void OnDrive( const grizzly_msgs::DriveConstPtr &msg); 00066 00068 std::string node_namespace_; 00069 std::string rl_joint_name_; 00070 std::string rr_joint_name_; 00071 std::string fl_joint_name_; 00072 std::string fr_joint_name_; 00073 std::string fa_joint_name_; 00074 std::string base_geom_name_; 00075 00077 float torque_; 00078 00079 ros::NodeHandle *rosnode_; 00080 00081 ros::Publisher encoder_pub_; 00082 ros::Publisher odom_pub_; 00083 ros::Publisher joint_state_pub_; 00084 ros::Subscriber drive_sub_; 00085 00086 physics::WorldPtr world_; 00087 physics::ModelPtr model_; 00088 sensors::SensorPtr parent_sensor_; 00089 00091 grizzly_msgs::Drive wheel_ang_vel_; 00092 00093 // Simulation time of the last update 00094 common::Time prev_update_time_; 00095 common::Time last_cmd_vel_time_; 00096 00097 bool set_joints_[5]; 00098 physics::JointPtr joints_[5]; 00099 physics::CollisionPtr base_geom_; 00100 00101 tf::TransformBroadcaster transform_broadcaster_; 00102 sensor_msgs::JointState js_; 00103 00104 void spin(); 00105 boost::thread *spinner_thread_; 00106 event::ConnectionPtr contact_event_; 00107 00108 // Pointer to the update event connection 00109 event::ConnectionPtr updateConnection; 00110 }; 00111 } 00112 #endif