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;
}
