void MasterController::slaveOdometryCallback(const nav_msgs::Odometry::ConstPtr& msg) { // Pose slave Eigen::Matrix<double,3,1> euler=Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z).matrix().eulerAngles(2, 1, 0); double yaw = euler(0,0); double pitch = euler(1,0); double roll = euler(2,0); if(!init_slave_readings) { previous_pose_slave << msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, roll-previous_pose_slave(3,0), pitch-previous_pose_slave(4,0), yaw; // should be relative // std::cout << "previous_pose_slave:" << previous_pose_slave(5,0) << " yaw:" << yaw << std::endl; // std::cout << "yaw:" << yaw << " yaw previous:" << yaw_slave_previous << std::endl; yaw_slave_previous=yaw; init_slave_readings=true; return; } else { // lastPositionUpdate = ros::Time::now().toSec(); current_pose_slave << msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, roll-previous_pose_slave(3,0), pitch-previous_pose_slave(4,0), yaw_slave_previous; // should be relative // std::cout << "current_pose_slave:" << current_pose_slave(5,0) << " yaw_slave_previous:" << yaw_slave_previous << std::endl; // std::cout << current_pose_slave(0,0) << std::endl ; // std::cout << current_pose_slave(1,0) << std::endl ; // std::cout << current_pose_slave(2,0) << std::endl ; // std::cout << current_pose_slave(3,0) << std::endl ; // std::cout << current_pose_slave(4,0) << std::endl ; // std::cout << current_pose_slave(5,0) << std::endl ; // double test = current_pose_slave(5,0) ; // std::cout << "yaw:" << yaw << " yaw previous:" << yaw_slave_previous << std::endl; // std::cout << "current_pose_slave:" << test << " previous_pose_slave previous:" << previous_pose_slave(5,0) << std::endl; // std::cout << "yaw TO DEG:" << yaw*RAD_TO_DEG << " yaw previous TO DEG:" << yaw_slave_previous * RAD_TO_DEG << std::endl; // std::cout << "current_pose_slave TO DEG (((:" << current_pose_slave(5,0)*RAD_TO_DEG << " previous_pose_slave previous TO DEG )))):" << previous_pose_slave(5,0) * RAD_TO_DEG << std::endl; yaw_slave_previous=yaw; } current_velocity_slave << msg->twist.twist.linear.x, msg->twist.twist.linear.y, msg->twist.twist.linear.z, msg->twist.twist.angular.x, msg->twist.twist.angular.y, msg->twist.twist.angular.z; slave_new_readings=true; feedback(); previous_pose_slave=current_pose_slave; }
// SLAVE MEASUREMENTS void SlaveController::slaveOdometryCallback(const nav_msgs::Odometry::ConstPtr& msg) { // Pose slave Eigen::Matrix<double,3,1> euler=Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z).matrix().eulerAngles(2, 1, 0); double yaw = euler(0,0); double pitch = euler(1,0); double roll = euler(2,0); if(!init_slave_readings) { previous_pose_slave << msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, roll-previous_pose_slave(3,0), pitch-previous_pose_slave(4,0), yaw; // should be relative //std::cout << "yaw:" << yaw << " yaw previous:" << yaw_slave_previous << std::endl; yaw_slave_previous=yaw; init_slave_readings=true; return; } else { current_pose_slave << msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, roll-previous_pose_slave(3,0), pitch-previous_pose_slave(4,0), yaw-yaw_slave_previous; // should be relative //std::cout << "yaw:" << yaw << " yaw previous:" << yaw_slave_previous << std::endl; yaw_slave_previous=yaw; } current_velocity_slave << msg->twist.twist.linear.x, msg->twist.twist.linear.y, msg->twist.twist.linear.z, msg->twist.twist.angular.x, msg->twist.twist.angular.y, msg->twist.twist.angular.z; slave_new_readings=true; feedback(); previous_pose_slave=current_pose_slave; }