| 279 | } |
| 280 | |
| 281 | void NetworkControl::odom_callback(const nav_msgs::Odometry::ConstPtr &odom) |
| 282 | { |
| 283 | cur_yaw_ = tf::getYaw(odom->pose.pose.orientation); |
| 284 | cur_vel_ = Eigen::Vector3d(odom->twist.twist.linear.x, odom->twist.twist.linear.y, odom->twist.twist.linear.z); |
| 285 | |
| 286 | cur_pos_ = Eigen::Vector3d(odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z); |
| 287 | cur_att_.w() = odom->pose.pose.orientation.w; |
| 288 | cur_att_.x() = odom->pose.pose.orientation.x; |
| 289 | cur_att_.y() = odom->pose.pose.orientation.y; |
| 290 | cur_att_.z() = odom->pose.pose.orientation.z; |
| 291 | |
| 292 | // if(!is_simulation_) |
| 293 | // cur_acc_ = Eigen::Vector3d(odom->twist.twist.angular.x, odom->twist.twist.angular.y, odom->twist.twist.angular.z); |
| 294 | |
| 295 | so3_controller_.setPosition(cur_pos_); |
| 296 | so3_controller_.setVelocity(cur_vel_); |
| 297 | if (!state_init_) |
| 298 | ROS_INFO("Odom Recived! Ready to TakeOff..."); |
| 299 | state_init_ = true; |
| 300 | } |
| 301 | |
| 302 | void NetworkControl::imu_callback(const sensor_msgs::Imu &imu) |
| 303 | { |
nothing calls this directly
no test coverage detected