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 #include <hector_quadrotor_controller/quadrotor_hardware_gazebo.h>
00030
00031 #include <geometry_msgs/WrenchStamped.h>
00032
00033 namespace hector_quadrotor_controller_gazebo {
00034
00035 QuadrotorHardwareSim::QuadrotorHardwareSim()
00036 {
00037 this->registerInterface(static_cast<QuadrotorInterface *>(this));
00038
00039 wrench_output_ = addInput<WrenchCommandHandle>("wrench");
00040 motor_output_ = addInput<MotorCommandHandle>("motor");
00041 }
00042
00043 QuadrotorHardwareSim::~QuadrotorHardwareSim()
00044 {
00045
00046 }
00047
00048 bool QuadrotorHardwareSim::initSim(
00049 const std::string& robot_namespace,
00050 ros::NodeHandle model_nh,
00051 gazebo::physics::ModelPtr parent_model,
00052 const urdf::Model *const urdf_model,
00053 std::vector<transmission_interface::TransmissionInfo> transmissions)
00054 {
00055 ros::NodeHandle param_nh(model_nh, "controller");
00056
00057
00058 model_ = parent_model;
00059 link_ = model_->GetLink();
00060 physics_ = model_->GetWorld()->GetPhysicsEngine();
00061
00062 model_nh.param<std::string>("world_frame", world_frame_, "world");
00063 model_nh.param<std::string>("base_link_frame", base_link_frame_, "base_link");
00064
00065
00066 std::string state_topic;
00067 param_nh.getParam("state_topic", state_topic);
00068 if (!state_topic.empty()) {
00069 ros::SubscribeOptions ops = ros::SubscribeOptions::create<nav_msgs::Odometry>(state_topic, 1, boost::bind(&QuadrotorHardwareSim::stateCallback, this, _1), ros::VoidConstPtr(), &callback_queue_);
00070 subscriber_state_ = model_nh.subscribe(ops);
00071
00072 gzlog << "[hector_quadrotor_controller_gazebo] Using topic '" << subscriber_state_.getTopic() << "' as state input for control" << std::endl;
00073 } else {
00074 gzlog << "[hector_quadrotor_controller_gazebo] Using ground truth from Gazebo as state input for control" << std::endl;
00075 }
00076
00077
00078 std::string imu_topic;
00079 param_nh.getParam("imu_topic", imu_topic);
00080 if (!imu_topic.empty()) {
00081 ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::Imu>(imu_topic, 1, boost::bind(&QuadrotorHardwareSim::imuCallback, this, _1), ros::VoidConstPtr(), &callback_queue_);
00082 subscriber_imu_ = model_nh.subscribe(ops);
00083 gzlog << "[hector_quadrotor_controller_gazebo] Using topic '" << subscriber_imu_.getTopic() << "' as imu input for control" << std::endl;
00084 } else {
00085 gzlog << "[hector_quadrotor_controller_gazebo] Using ground truth from Gazebo as imu input for control" << std::endl;
00086 }
00087
00088
00089 {
00090 ros::SubscribeOptions ops = ros::SubscribeOptions::create<hector_uav_msgs::MotorStatus>("motor_status", 1, boost::bind(&QuadrotorHardwareSim::motorStatusCallback, this, _1), ros::VoidConstPtr(), &callback_queue_);
00091 subscriber_motor_status_ = model_nh.subscribe(ops);
00092 }
00093
00094
00095 {
00096 ros::AdvertiseOptions ops = ros::AdvertiseOptions::create<geometry_msgs::WrenchStamped>("command/wrench", 1, ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(), ros::VoidConstPtr(), &callback_queue_);
00097 publisher_wrench_command_ = model_nh.advertise(ops);
00098 }
00099
00100
00101 {
00102 ros::AdvertiseOptions ops = ros::AdvertiseOptions::create<hector_uav_msgs::MotorCommand>("command/motor", 1, ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(), ros::VoidConstPtr(), &callback_queue_);
00103 publisher_motor_command_ = model_nh.advertise(ops);
00104 }
00105
00106 return true;
00107 }
00108
00109 bool QuadrotorHardwareSim::getMassAndInertia(double &mass, double inertia[3]) {
00110 if (!link_) return false;
00111 mass = link_->GetInertial()->GetMass();
00112 gazebo::math::Vector3 Inertia = link_->GetInertial()->GetPrincipalMoments();
00113 inertia[0] = Inertia.x;
00114 inertia[1] = Inertia.y;
00115 inertia[2] = Inertia.z;
00116 return true;
00117 }
00118
00119 void QuadrotorHardwareSim::stateCallback(const nav_msgs::OdometryConstPtr &state) {
00120
00121 if (!header_.stamp.isZero() && !state->header.stamp.isZero()) {
00122 const double acceleration_time_constant = 0.1;
00123 double dt((state->header.stamp - header_.stamp).toSec());
00124 if (dt > 0.0) {
00125 acceleration_.x = ((state->twist.twist.linear.x - twist_.linear.x) + acceleration_time_constant * acceleration_.x) / (dt + acceleration_time_constant);
00126 acceleration_.y = ((state->twist.twist.linear.y - twist_.linear.y) + acceleration_time_constant * acceleration_.y) / (dt + acceleration_time_constant);
00127 acceleration_.z = ((state->twist.twist.linear.z - twist_.linear.z) + acceleration_time_constant * acceleration_.z) / (dt + acceleration_time_constant);
00128 }
00129 }
00130
00131 header_ = state->header;
00132 pose_ = state->pose.pose;
00133 twist_ = state->twist.twist;
00134 }
00135
00136 void QuadrotorHardwareSim::imuCallback(const sensor_msgs::ImuConstPtr &imu) {
00137 imu_ = *imu;
00138 }
00139
00140 void QuadrotorHardwareSim::motorStatusCallback(const hector_uav_msgs::MotorStatusConstPtr &motor_status) {
00141 motor_status_ = *motor_status;
00142 }
00143
00144 void QuadrotorHardwareSim::readSim(ros::Time time, ros::Duration period)
00145 {
00146
00147 callback_queue_.callAvailable();
00148
00149
00150 const double acceleration_time_constant = 0.1;
00151 gz_pose_ = link_->GetWorldPose();
00152 gz_acceleration_ = ((link_->GetWorldLinearVel() - gz_velocity_) + acceleration_time_constant * gz_acceleration_) / (period.toSec() + acceleration_time_constant);
00153 gz_velocity_ = link_->GetWorldLinearVel();
00154 gz_angular_velocity_ = link_->GetWorldAngularVel();
00155
00156 if (!subscriber_state_) {
00157 header_.frame_id = world_frame_;
00158 header_.stamp = time;
00159 pose_.position.x = gz_pose_.pos.x;
00160 pose_.position.y = gz_pose_.pos.y;
00161 pose_.position.z = gz_pose_.pos.z;
00162 pose_.orientation.w = gz_pose_.rot.w;
00163 pose_.orientation.x = gz_pose_.rot.x;
00164 pose_.orientation.y = gz_pose_.rot.y;
00165 pose_.orientation.z = gz_pose_.rot.z;
00166 twist_.linear.x = gz_velocity_.x;
00167 twist_.linear.y = gz_velocity_.y;
00168 twist_.linear.z = gz_velocity_.z;
00169 twist_.angular.x = gz_angular_velocity_.x;
00170 twist_.angular.y = gz_angular_velocity_.y;
00171 twist_.angular.z = gz_angular_velocity_.z;
00172 acceleration_.x = gz_acceleration_.x;
00173 acceleration_.y = gz_acceleration_.y;
00174 acceleration_.z = gz_acceleration_.z;
00175 }
00176
00177 if (!subscriber_imu_) {
00178 imu_.orientation.w = gz_pose_.rot.w;
00179 imu_.orientation.x = gz_pose_.rot.x;
00180 imu_.orientation.y = gz_pose_.rot.y;
00181 imu_.orientation.z = gz_pose_.rot.z;
00182
00183 gazebo::math::Vector3 gz_angular_velocity_body = gz_pose_.rot.RotateVectorReverse(gz_angular_velocity_);
00184 imu_.angular_velocity.x = gz_angular_velocity_body.x;
00185 imu_.angular_velocity.y = gz_angular_velocity_body.y;
00186 imu_.angular_velocity.z = gz_angular_velocity_body.z;
00187
00188 gazebo::math::Vector3 gz_linear_acceleration_body = gz_pose_.rot.RotateVectorReverse(gz_acceleration_ - physics_->GetGravity());
00189 imu_.linear_acceleration.x = gz_linear_acceleration_body.x;
00190 imu_.linear_acceleration.y = gz_linear_acceleration_body.y;
00191 imu_.linear_acceleration.z = gz_linear_acceleration_body.z;
00192 }
00193 }
00194
00195 void QuadrotorHardwareSim::writeSim(ros::Time time, ros::Duration period)
00196 {
00197 bool result_written = false;
00198
00199 if (motor_output_->connected() && motor_output_->enabled()) {
00200 publisher_motor_command_.publish(motor_output_->getCommand());
00201 result_written = true;
00202 }
00203
00204 if (wrench_output_->connected() && wrench_output_->enabled()) {
00205 geometry_msgs::WrenchStamped wrench;
00206 wrench.header.stamp = time;
00207 wrench.header.frame_id = base_link_frame_;
00208 wrench.wrench = wrench_output_->getCommand();
00209 publisher_wrench_command_.publish(wrench);
00210
00211 if (!result_written) {
00212 gazebo::math::Vector3 force(wrench.wrench.force.x, wrench.wrench.force.y, wrench.wrench.force.z);
00213 gazebo::math::Vector3 torque(wrench.wrench.torque.x, wrench.wrench.torque.y, wrench.wrench.torque.z);
00214 link_->AddRelativeForce(force);
00215 link_->AddRelativeTorque(torque - link_->GetInertial()->GetCoG().Cross(force));
00216 }
00217 }
00218 }
00219
00220 }
00221
00222 #include <pluginlib/class_list_macros.h>
00223 PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller_gazebo::QuadrotorHardwareSim, gazebo_ros_control::RobotHWSim)