00001 #ifndef GAZEBO_ROS_CREATE_H 00002 #define GAZEBO_ROS_CREATE_H 00003 00004 #include <gazebo/Controller.hh> 00005 #include <gazebo/Model.hh> 00006 #include <gazebo/Geom.hh> 00007 #include <gazebo/Time.hh> 00008 #include <gazebo/RaySensor.hh> 00009 00010 #include <nav_msgs/Odometry.h> 00011 #include <geometry_msgs/TwistWithCovariance.h> 00012 #include <geometry_msgs/PoseWithCovariance.h> 00013 00014 #include <tf/transform_broadcaster.h> 00015 #include <ros/ros.h> 00016 00017 namespace gazebo 00018 { 00019 class GazeboRosCreate : public Controller 00020 { 00021 public: 00022 GazeboRosCreate( gazebo::Entity *parent ); 00023 virtual ~GazeboRosCreate(); 00024 00025 virtual void LoadChild( XMLConfigNode *node ); 00026 virtual void InitChild(); 00027 virtual void FiniChild(); 00028 virtual void UpdateChild(); 00029 00030 private: 00031 00032 void UpdateSensors(); 00033 void OnContact(const gazebo::Contact &contact); 00034 void OnCmdVel( const geometry_msgs::TwistConstPtr &msg); 00035 00036 ros::NodeHandle *rosnode_; 00037 //ros::Service operating_mode_srv_; 00038 //ros::Service digital_output_srv_; 00039 00040 ros::Publisher sensor_state_pub_; 00041 ros::Publisher odom_pub_; 00042 ros::Publisher joint_state_pub_; 00043 00044 ros::Subscriber cmd_vel_sub_; 00045 00046 ParamT<std::string> *node_namespaceP_; 00047 ParamT<std::string> *left_wheel_joint_nameP_; 00048 ParamT<std::string> *right_wheel_joint_nameP_; 00049 ParamT<std::string> *front_castor_joint_nameP_; 00050 ParamT<std::string> *rear_castor_joint_nameP_; 00051 ParamT<std::string> *base_geom_nameP_; 00052 00054 ParamT<float> *wheel_sepP_; 00055 00057 ParamT<float> *wheel_diamP_; 00058 00060 ParamT<float> *torqueP_; 00061 00062 Model *my_parent_; 00063 00065 float *wheel_speed_; 00066 00067 // Simulation time of the last update 00068 Time prev_update_time_; 00069 00070 float odom_pose_[3]; 00071 float odom_vel_[3]; 00072 00073 Joint *joints_[4]; 00074 Geom *base_geom_; 00075 00076 RaySensor *left_cliff_sensor_; 00077 RaySensor *leftfront_cliff_sensor_; 00078 RaySensor *rightfront_cliff_sensor_; 00079 RaySensor *right_cliff_sensor_; 00080 RaySensor *wall_sensor_; 00081 00082 tf::TransformBroadcaster transform_broadcaster_; 00083 sensor_msgs::JointState js_; 00084 00085 turtlebot_node::TurtlebotSensorState sensor_state_; 00086 }; 00087 } 00088 #endif