| 21 | } |
| 22 | |
| 23 | void pubOdom() |
| 24 | { |
| 25 | nav_msgs::Odometry odom; |
| 26 | odom.header.stamp = ros::Time::now(); |
| 27 | odom.header.frame_id = "world"; |
| 28 | odom.child_frame_id = std::to_string(drone_id_); |
| 29 | |
| 30 | if(rcv_cmd) |
| 31 | { |
| 32 | odom.pose.pose.position.x = _cmd.position.x; |
| 33 | odom.pose.pose.position.y = _cmd.position.y; |
| 34 | odom.pose.pose.position.z = _cmd.position.z; |
| 35 | |
| 36 | // if(drone_id_ > 1) { |
| 37 | // odom.pose.pose.position.x += _init_x; |
| 38 | // odom.pose.pose.position.y += _init_y; |
| 39 | // } |
| 40 | |
| 41 | Eigen::Vector3d alpha = Eigen::Vector3d(_cmd.acceleration.x, _cmd.acceleration.y, _cmd.acceleration.z) + 9.8*Eigen::Vector3d(0,0,1); |
| 42 | Eigen::Vector3d xC(cos(_cmd.yaw), sin(_cmd.yaw), 0); |
| 43 | Eigen::Vector3d yC(-sin(_cmd.yaw), cos(_cmd.yaw), 0); |
| 44 | Eigen::Vector3d xB = (yC.cross(alpha)).normalized(); |
| 45 | Eigen::Vector3d yB = (alpha.cross(xB)).normalized(); |
| 46 | Eigen::Vector3d zB = xB.cross(yB); |
| 47 | Eigen::Matrix3d R; |
| 48 | R.col(0) = xB; |
| 49 | R.col(1) = yB; |
| 50 | R.col(2) = zB; |
| 51 | Eigen::Quaterniond q(R); |
| 52 | odom.pose.pose.orientation.w = q.w(); |
| 53 | odom.pose.pose.orientation.x = q.x(); |
| 54 | odom.pose.pose.orientation.y = q.y(); |
| 55 | odom.pose.pose.orientation.z = q.z(); |
| 56 | |
| 57 | odom.twist.twist.linear.x = _cmd.velocity.x; |
| 58 | odom.twist.twist.linear.y = _cmd.velocity.y; |
| 59 | odom.twist.twist.linear.z = _cmd.velocity.z; |
| 60 | |
| 61 | odom.twist.twist.angular.x = _cmd.acceleration.x; |
| 62 | odom.twist.twist.angular.y = _cmd.acceleration.y; |
| 63 | odom.twist.twist.angular.z = _cmd.acceleration.z; |
| 64 | } |
| 65 | else |
| 66 | { |
| 67 | odom.pose.pose.position.x = _init_x; |
| 68 | odom.pose.pose.position.y = _init_y; |
| 69 | odom.pose.pose.position.z = _init_z; |
| 70 | |
| 71 | odom.pose.pose.orientation.w = 1; |
| 72 | odom.pose.pose.orientation.x = 0; |
| 73 | odom.pose.pose.orientation.y = 0; |
| 74 | odom.pose.pose.orientation.z = 0; |
| 75 | |
| 76 | odom.twist.twist.linear.x = 0.0; |
| 77 | odom.twist.twist.linear.y = 0.0; |
| 78 | odom.twist.twist.linear.z = 0.0; |
| 79 | |
| 80 | odom.twist.twist.angular.x = 0.0; |