| 143 | } |
| 144 | |
| 145 | void SO3ControlNodelet::onInit(void) { |
| 146 | ros::NodeHandle n(getPrivateNodeHandle()); |
| 147 | |
| 148 | std::string quadrotor_name; |
| 149 | n.param("quadrotor_name", quadrotor_name, std::string("quadrotor")); |
| 150 | frame_id_ = "/" + quadrotor_name; |
| 151 | |
| 152 | double mass; |
| 153 | n.param("mass", mass, 0.74); |
| 154 | controller_.setMass(mass); |
| 155 | |
| 156 | n.param("use_external_yaw", use_external_yaw_, true); |
| 157 | |
| 158 | n.param("gains/rot/x", kR_[0], 1.5); |
| 159 | n.param("gains/rot/y", kR_[1], 1.5); |
| 160 | n.param("gains/rot/z", kR_[2], 1.0); |
| 161 | n.param("gains/ang/x", kOm_[0], 0.13); |
| 162 | n.param("gains/ang/y", kOm_[1], 0.13); |
| 163 | n.param("gains/ang/z", kOm_[2], 0.1); |
| 164 | |
| 165 | n.param("corrections/z", corrections_[0], 0.0); |
| 166 | n.param("corrections/r", corrections_[1], 0.0); |
| 167 | n.param("corrections/p", corrections_[2], 0.0); |
| 168 | |
| 169 | so3_command_pub_ = n.advertise<quadrotor_msgs::SO3Command>("so3_cmd", 10); |
| 170 | |
| 171 | odom_sub_ = n.subscribe("odom", 10, &SO3ControlNodelet::odom_callback, this, |
| 172 | ros::TransportHints().tcpNoDelay()); |
| 173 | position_cmd_sub_ = n.subscribe("position_cmd", 10, &SO3ControlNodelet::position_cmd_callback, this, |
| 174 | ros::TransportHints().tcpNoDelay()); |
| 175 | |
| 176 | enable_motors_sub_ = n.subscribe("motors", 2, &SO3ControlNodelet::enable_motors_callback, this, |
| 177 | ros::TransportHints().tcpNoDelay()); |
| 178 | corrections_sub_ = n.subscribe("corrections", 10, &SO3ControlNodelet::corrections_callback, this, |
| 179 | ros::TransportHints().tcpNoDelay()); |
| 180 | |
| 181 | imu_sub_ = |
| 182 | n.subscribe("imu", 10, &SO3ControlNodelet::imu_callback, this, ros::TransportHints().tcpNoDelay()); |
| 183 | } |
| 184 | |
| 185 | #include <pluginlib/class_list_macros.h> |
| 186 | // PLUGINLIB_DECLARE_CLASS(so3_control, SO3ControlNodelet, SO3ControlNodelet, nodelet::Nodelet); |