00001 #ifndef GAZEBO_ROS_CREATE_H 00002 #define GAZEBO_ROS_CREATE_H 00003 00004 #include <gazebo/msgs/msgs.hh> 00005 #include <gazebo/physics/physics.hh> 00006 #include <gazebo/physics/PhysicsTypes.hh> 00007 #include <gazebo/sensors/SensorTypes.hh> 00008 #include <gazebo/transport/TransportTypes.hh> 00009 #include <gazebo/transport/transport.hh> 00010 #include <gazebo/common/Time.hh> 00011 #include <gazebo/common/Plugin.hh> 00012 #include <gazebo/common/Events.hh> 00013 00014 #include <nav_msgs/Odometry.h> 00015 #include <geometry_msgs/TwistWithCovariance.h> 00016 #include <geometry_msgs/PoseWithCovariance.h> 00017 #include <tf/transform_broadcaster.h> 00018 #include <ros/ros.h> 00019 00020 namespace gazebo 00021 { 00022 class GazeboRosCreate : public ModelPlugin 00023 { 00024 public: 00025 GazeboRosCreate(); 00026 virtual ~GazeboRosCreate(); 00027 00028 virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf ); 00029 00030 virtual void UpdateChild(); 00031 00032 private: 00033 00034 void UpdateSensors(); 00035 void OnContact(ConstContactsPtr &contact); 00036 void OnCmdVel( const geometry_msgs::TwistConstPtr &msg); 00037 00038 00040 std::string node_namespace_; 00041 std::string left_wheel_joint_name_; 00042 std::string right_wheel_joint_name_; 00043 std::string front_castor_joint_name_; 00044 std::string rear_castor_joint_name_; 00045 std::string base_geom_name_; 00046 00048 float wheel_sep_; 00049 00051 float wheel_diam_; 00052 00054 float torque_; 00055 00056 00057 ros::NodeHandle *rosnode_; 00058 //ros::Service operating_mode_srv_; 00059 //ros::Service digital_output_srv_; 00060 00061 ros::Publisher sensor_state_pub_; 00062 ros::Publisher odom_pub_; 00063 ros::Publisher joint_state_pub_; 00064 00065 ros::Subscriber cmd_vel_sub_; 00066 00067 physics::WorldPtr my_world_; 00068 physics::ModelPtr my_parent_; 00069 00071 float *wheel_speed_; 00072 00073 // Simulation time of the last update 00074 common::Time prev_update_time_; 00075 common::Time last_cmd_vel_time_; 00076 00077 float odom_pose_[3]; 00078 float odom_vel_[3]; 00079 00080 bool set_joints_[4]; 00081 physics::JointPtr joints_[4]; 00082 physics::CollisionPtr base_geom_; 00083 00084 sensors::RaySensorPtr left_cliff_sensor_; 00085 sensors::RaySensorPtr leftfront_cliff_sensor_; 00086 sensors::RaySensorPtr rightfront_cliff_sensor_; 00087 sensors::RaySensorPtr right_cliff_sensor_; 00088 sensors::RaySensorPtr wall_sensor_; 00089 00090 tf::TransformBroadcaster transform_broadcaster_; 00091 sensor_msgs::JointState js_; 00092 00093 create_node::TurtlebotSensorState sensor_state_; 00094 00095 void spin(); 00096 boost::thread *spinner_thread_; 00097 00098 //event::ConnectionPtr contact_event_; 00099 transport::NodePtr gazebo_node_; 00100 transport::SubscriberPtr contact_sub_; 00101 00102 // Pointer to the update event connection 00103 event::ConnectionPtr updateConnection; 00104 }; 00105 } 00106 #endif