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
00027
00028
00029 #ifndef HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_HARDWARE_GAZEBO_H
00030 #define HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_HARDWARE_GAZEBO_H
00031
00032 #include <gazebo_ros_control/robot_hw_sim.h>
00033 #include <hector_quadrotor_controller/quadrotor_interface.h>
00034
00035 #include <nav_msgs/Odometry.h>
00036 #include <sensor_msgs/Imu.h>
00037 #include <hector_uav_msgs/MotorStatus.h>
00038
00039 #include <ros/node_handle.h>
00040 #include <ros/callback_queue.h>
00041
00042 namespace hector_quadrotor_controller_gazebo {
00043
00044 using namespace hector_quadrotor_controller;
00045 using namespace hardware_interface;
00046 using namespace gazebo_ros_control;
00047
00048 class QuadrotorHardwareSim : public RobotHWSim, public QuadrotorInterface
00049 {
00050 public:
00051 QuadrotorHardwareSim();
00052 virtual ~QuadrotorHardwareSim();
00053
00054 virtual const ros::Time &getTimestamp() { return header_.stamp; }
00055
00056 virtual PoseHandlePtr getPose() { return PoseHandlePtr(new PoseHandle(this, &pose_)); }
00057 virtual TwistHandlePtr getTwist() { return TwistHandlePtr(new TwistHandle(this, &twist_)); }
00058 virtual AccelerationHandlePtr getAcceleration() { return AccelerationHandlePtr(new AccelerationHandle(this, &acceleration_)); }
00059 virtual ImuHandlePtr getSensorImu() { return ImuHandlePtr(new ImuHandle(this, &imu_)); }
00060 virtual MotorStatusHandlePtr getMotorStatus() { return MotorStatusHandlePtr(new MotorStatusHandle(this, &motor_status_)); }
00061
00062 virtual bool getMassAndInertia(double &mass, double inertia[3]);
00063
00064 virtual bool initSim(
00065 const std::string& robot_namespace,
00066 ros::NodeHandle model_nh,
00067 gazebo::physics::ModelPtr parent_model,
00068 const urdf::Model *const urdf_model,
00069 std::vector<transmission_interface::TransmissionInfo> transmissions);
00070
00071 virtual void readSim(ros::Time time, ros::Duration period);
00072
00073 virtual void writeSim(ros::Time time, ros::Duration period);
00074
00075 private:
00076 void stateCallback(const nav_msgs::OdometryConstPtr &state);
00077 void imuCallback(const sensor_msgs::ImuConstPtr &imu);
00078 void motorStatusCallback(const hector_uav_msgs::MotorStatusConstPtr &motor_status);
00079
00080 protected:
00081 std_msgs::Header header_;
00082 Pose pose_;
00083 Twist twist_;
00084 Vector3 acceleration_;
00085 Imu imu_;
00086 MotorStatus motor_status_;
00087 std::string base_link_frame_, world_frame_;
00088
00089 WrenchCommandHandlePtr wrench_output_;
00090 MotorCommandHandlePtr motor_output_;
00091
00092 gazebo::physics::ModelPtr model_;
00093 gazebo::physics::LinkPtr link_;
00094 gazebo::physics::PhysicsEnginePtr physics_;
00095
00096 gazebo::math::Pose gz_pose_;
00097 gazebo::math::Vector3 gz_velocity_, gz_acceleration_, gz_angular_velocity_;
00098
00099 ros::CallbackQueue callback_queue_;
00100 ros::Subscriber subscriber_state_;
00101 ros::Subscriber subscriber_imu_;
00102 ros::Subscriber subscriber_motor_status_;
00103 ros::Publisher publisher_wrench_command_;
00104 ros::Publisher publisher_motor_command_;
00105 };
00106
00107 }
00108
00109 #endif // HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_HARDWARE_GAZEBO_H