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