tf::Pose MotionModel::updateAction(tf::Pose& p)
{
double delta_rot1;
if (dxy < 0.01)
delta_rot1 = 0.0;
else
delta_rot1 = angle_diff(atan2(delta_y, delta_x), delta_yaw);
double delta_trans = dxy;
double delta_rot2 = angle_diff(delta_yaw, delta_rot1);
double delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)), fabs(angle_diff(delta_rot1, M_PI)));
double delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)), fabs(angle_diff(delta_rot2, M_PI)));
double delta_rot1_hat = angle_diff(delta_rot1, sampleGaussian(alpha1 * delta_rot1_noise*delta_rot1_noise +
alpha2 * delta_trans*delta_trans));
double delta_trans_hat = delta_trans - sampleGaussian(alpha3 * delta_trans*delta_trans +
alpha4 * delta_rot1_noise*delta_rot1_noise +
alpha4 * delta_rot2_noise*delta_rot2_noise);
double delta_rot2_hat = angle_diff(delta_rot2, sampleGaussian(alpha1 * delta_rot2_noise*delta_rot2_noise +
alpha2 * delta_trans*delta_trans));
delta_x = delta_trans_hat * cos(delta_rot1_hat);
delta_y = delta_trans_hat * sin(delta_rot1_hat);
delta_yaw = delta_rot1_hat + delta_rot2_hat;
tf::Pose noisy_pose(tf::createQuaternionFromRPY(0,0,delta_yaw),tf::Vector3(delta_x,delta_y,0));
tf::Transform base_to_global_ = tf::Transform(p.getRotation());
noisy_pose.setOrigin(base_to_global_ * noisy_pose.getOrigin());
p.setOrigin(p.getOrigin() + noisy_pose.getOrigin());
p.setRotation(p.getRotation() * noisy_pose.getRotation());
}
// Convert from CameraPose to tf::Pose (position and orientation)
inline void CameraPose2TFPose(const CameraPose& cameraPose, tf::Pose& pose)
{
cv::Vec4d orientation = cameraPose.GetOrientationQuaternion();
cv::Vec3d position = cameraPose.GetPosition();
pose.setOrigin(tf::Vector3(position[0], position[1], position[2]));
pose.setRotation(tf::Quaternion(orientation[1], orientation[2], orientation[3], orientation[0] )); // q = (x,y,z,w)
}
void poseKDLToTF(const KDL::Frame& k, tf::Pose& t)
{
t.setOrigin(tf::Vector3(k.p[0], k.p[1], k.p[2]));
t.setBasis(tf::Matrix3x3(k.M.data[0], k.M.data[1], k.M.data[2],
k.M.data[3], k.M.data[4], k.M.data[5],
k.M.data[6], k.M.data[7], k.M.data[8]));
}
void toPose(const MathLib::Matrix4& mat4, tf::Pose& pose) {
MathLib::Matrix3 m1 = mat4.GetOrientation();
MathLib::Vector3 v1 = m1.GetRotationAxis();
tf::Vector3 ax(v1(0), v1(1), v1(2));
pose.setRotation(tf::Quaternion(ax, m1.GetRotationAngle()));
v1.Set(mat4.GetTranslation());
pose.setOrigin(tf::Vector3(v1(0),v1(1),v1(2)));
}
bool MarkerSearcherComputation::getNextSearchPose(tf::Pose& returnPose) {
if(poseReturned) {
return false;
}
returnPose.setOrigin(nextSearchPose.getOrigin());
returnPose.setRotation(nextSearchPose.getRotation());
poseReturned = true;
return true;
}
void toTfPose(const geometry_msgs::Pose& msg_pose, tf::Pose& tf_pose)
{
tf_pose.setOrigin(tf::Vector3(msg_pose.position.x,
msg_pose.position.y,
msg_pose.position.z));
tf_pose.setRotation(tf::Quaternion(msg_pose.orientation.x,
msg_pose.orientation.y,
msg_pose.orientation.z,
msg_pose.orientation.w));
}
// Callback for the desired cartesian pose
void cartCallback(const geometry_msgs::PoseStampedConstPtr& msg) {
ROS_INFO_STREAM("Received Position Command");
const geometry_msgs::PoseStamped* data = msg.get();
des_ee_pose.setOrigin(tf::Vector3(data->pose.position.x,data->pose.position.y,data->pose.position.z));
des_ee_pose.setRotation(tf::Quaternion(data->pose.orientation.x,data->pose.orientation.y,data->pose.orientation.z,data->pose.orientation.w));
ROS_INFO_STREAM_THROTTLE(1, "Received Position: "<<des_ee_pose.getOrigin().x()<<","<<des_ee_pose.getOrigin().y()<<","<<des_ee_pose.getOrigin().z());
if(bOrientCtrl) {
tf::Quaternion q = des_ee_pose.getRotation();
ROS_INFO_STREAM_THROTTLE(1, "Received Orientation: "<<q.x()<<","<<q.y()<<","<<q.z()<<","<<q.w());
}
}
void RTKRobotArm::getEEPose(tf::Pose& pose) {
if(mRobot) {
//Collect ee pose from RTK
MathLib::Vector eeq(4);
Matrix3 tmp = mRobot->GetReferenceFrame(nEndEffectorId).GetOrient();
tmp.GetQuaternionRepresentation(eeq);
MathLib::Vector3 eep = mRobot->GetReferenceFrame(nEndEffectorId).GetOrigin();
// Convert to tf
pose.setRotation(tf::Quaternion(eeq[1], eeq[2], eeq[3], eeq[0]));
pose.setOrigin(tf::Vector3(eep[0], eep[1], eep[2]));
} else{
ROS_WARN("RTK Robot not initialized");
}
}
void StepCostKey::mirrorPoseOnXPlane(tf::Pose &mirror, const tf::Pose &orig) const
{
mirror.setBasis(orig.getBasis());
tf::Matrix3x3& basis = mirror.getBasis();
basis[0][1] = -basis[0][1];
basis[1][0] = -basis[1][0];
basis[2][1] = -basis[2][1];
mirror.setOrigin(orig.getOrigin());
tf::Vector3& origin = mirror.getOrigin();
origin[1] = -origin[1];
// double r, p, y;
// tf::Matrix3x3(orig.getRotation()).getRPY(r, p, y);
// mirror.setRotation(tf::createQuaternionFromRPY(-r, p, -y));
// tf::Vector3 v = orig.getOrigin();
// v.setY(-v.getY());
// mirror.setOrigin(v);
// tfScalar m[16];
// ROS_INFO("------------------");
// mirror.getOpenGLMatrix(m);
// ROS_INFO("%f %f %f %f", m[0], m[4], m[8], m[12]);
// ROS_INFO("%f %f %f %f", m[1], m[5], m[9], m[13]);
// ROS_INFO("%f %f %f %f", m[2], m[6], m[10], m[14]);
// ROS_INFO("%f %f %f %f", m[3], m[7], m[11], m[15]);
// ROS_INFO("-");
// orig.getOpenGLMatrix(m);
// ROS_INFO("%f %f %f %f", m[0], m[4], m[8], m[12]);
// ROS_INFO("%f %f %f %f", m[1], m[5], m[9], m[13]);
// ROS_INFO("%f %f %f %f", m[2], m[6], m[10], m[14]);
// ROS_INFO("%f %f %f %f", m[3], m[7], m[11], m[15]);
// ROS_INFO("-");
// ROS_INFO("%f %f %f %f", m[0], -m[4], m[8], m[12]);
// ROS_INFO("%f %f %f %f", -m[1], m[5], m[9], -m[13]);
// ROS_INFO("%f %f %f %f", m[2], -m[6], m[10], m[14]);
// ROS_INFO("%f %f %f %f", m[3], m[7], m[11], m[15]);
}
tf::Pose MotionModel::drawFromMotion(tf::Pose& p)
{
double sxy=0.3*srr;
delta_x+=sampleGaussian(srr*fabs(delta_x)+str*fabs(delta_yaw)+sxy*fabs(delta_y));
delta_y+=sampleGaussian(srr*fabs(delta_y)+str*fabs(delta_yaw)+sxy*fabs(delta_x));
delta_yaw+=sampleGaussian(stt*fabs(delta_yaw)+srt*dxy);
delta_yaw=fmod(delta_yaw, 2*M_PI);
if (delta_yaw>M_PI)
delta_yaw-=2*M_PI;
tf::Pose noisy_pose(tf::createQuaternionFromRPY(0,0,delta_yaw),tf::Vector3(delta_x,delta_y,0));
tf::Transform base_to_global_ = tf::Transform(p.getRotation());
noisy_pose.setOrigin(base_to_global_ * noisy_pose.getOrigin());
p.setOrigin(p.getOrigin() + noisy_pose.getOrigin());
p.setRotation(p.getRotation() * noisy_pose.getRotation());
return p;
}
void NaoqiJointStates::run()
{
ros::Rate r(m_rate);
ros::Time stamp1;
ros::Time stamp2;
ros::Time stamp;
std::vector<float> odomData;
float odomX, odomY, odomZ, odomWX, odomWY, odomWZ;
std::vector<float> memData;
std::vector<float> positionData;
ROS_INFO("Staring main loop. ros::ok() is %d nh.ok() is %d",ros::ok(),m_nh.ok());
while(ros::ok() )
{
r.sleep();
ros::spinOnce();
stamp1 = ros::Time::now();
try
{
memData = m_memoryProxy->getListData(m_dataNamesList);
// {SPACE_TORSO = 0, SPACE_WORLD = 1, SPACE_NAO = 2}. (second argument)
odomData = m_motionProxy->getPosition("Torso", 1, true);
positionData = m_motionProxy->getAngles("Body", true);
}
catch (const AL::ALError & e)
{
ROS_ERROR( "Error accessing ALMemory, exiting...");
ROS_ERROR( "%s", e.what() );
//ros.signal_shutdown("No NaoQI available anymore");
}
stamp2 = ros::Time::now();
//ROS_DEBUG("dt is %f",(stamp2-stamp1).toSec()); % dt is typically around 1/1000 sec
// TODO: Something smarter than this..
stamp = stamp1 + ros::Duration((stamp2-stamp1).toSec()/2.0);
/******************************************************************
* IMU
*****************************************************************/
if (memData.size() != m_dataNamesList.getSize())
{
ROS_ERROR("memData length %zu does not match expected length %u",memData.size(),m_dataNamesList.getSize() );
continue;
}
// IMU data:
m_torsoIMU.header.stamp = stamp;
float angleX = memData[1];
float angleY = memData[2];
float angleZ = memData[3];
float gyroX = memData[4];
float gyroY = memData[5];
float gyroZ = memData[6];
float accX = memData[7];
float accY = memData[8];
float accZ = memData[9];
m_torsoIMU.orientation = tf::createQuaternionMsgFromRollPitchYaw(
angleX,
angleY,
angleZ); // yaw currently always 0
m_torsoIMU.angular_velocity.x = gyroX;
m_torsoIMU.angular_velocity.y = gyroY;
m_torsoIMU.angular_velocity.z = gyroZ; // currently always 0
m_torsoIMU.linear_acceleration.x = accX;
m_torsoIMU.linear_acceleration.y = accY;
m_torsoIMU.linear_acceleration.z = accZ;
// covariances unknown
// cf http://www.ros.org/doc/api/sensor_msgs/html/msg/Imu.html
m_torsoIMU.orientation_covariance[0] = -1;
m_torsoIMU.angular_velocity_covariance[0] = -1;
m_torsoIMU.linear_acceleration_covariance[0] = -1;
m_torsoIMUPub.publish(m_torsoIMU);
/******************************************************************
* Joint state
*****************************************************************/
m_jointState.header.stamp = stamp;
m_jointState.header.frame_id = m_baseFrameId;
m_jointState.position.resize(positionData.size());
for(unsigned i = 0; i<positionData.size(); ++i)
{
m_jointState.position[i] = positionData[i];
}
// simulated model misses some joints, we need to fill:
if (m_jointState.position.size() == 22)
{
m_jointState.position.insert(m_jointState.position.begin()+6,0.0);
m_jointState.position.insert(m_jointState.position.begin()+7,0.0);
m_jointState.position.push_back(0.0);
m_jointState.position.push_back(0.0);
}
m_jointStatePub.publish(m_jointState);
/******************************************************************
* Odometry
*****************************************************************/
if (!m_paused) {
// apply offset transformation:
tf::Pose transformedPose;
if (odomData.size()!=6)
{
ROS_ERROR( "Error getting odom data. length is %zu",odomData.size() );
continue;
}
double dt = (stamp.toSec() - m_lastOdomTime);
odomX = odomData[0];
odomY = odomData[1];
odomZ = odomData[2];
odomWX = odomData[3];
odomWY = odomData[4];
odomWZ = odomData[5];
tf::Quaternion q;
// roll and pitch from IMU, yaw from odometry:
if (m_useIMUAngles)
q.setRPY(angleX, angleY, odomWZ);
else
q.setRPY(odomWX, odomWY, odomWZ);
m_odomPose.setOrigin(tf::Vector3(odomX, odomY, odomZ));
m_odomPose.setRotation(q);
if(m_mustUpdateOffset) {
if(!m_isInitialized) {
if(m_initializeFromIMU) {
// Initialization from IMU: Take x, y, z, yaw from odometry, roll and pitch from IMU
m_targetPose.setOrigin(m_odomPose.getOrigin());
m_targetPose.setRotation(tf::createQuaternionFromRPY(angleX, angleY, odomWZ));
} else if(m_initializeFromOdometry) {
m_targetPose.setOrigin(m_odomPose.getOrigin());
m_targetPose.setRotation(tf::createQuaternionFromRPY(odomWX, odomWY, odomWZ));
}
m_isInitialized = true;
} else {
// Overwrite target pitch and roll angles with IMU data
const double target_yaw = tf::getYaw(m_targetPose.getRotation());
if(m_initializeFromIMU) {
m_targetPose.setRotation(tf::createQuaternionFromRPY(angleX, angleY, target_yaw));
} else if(m_initializeFromOdometry){
m_targetPose.setRotation(tf::createQuaternionFromRPY(odomWX, odomWY, target_yaw));
}
}
m_odomOffset = m_targetPose * m_odomPose.inverse();
transformedPose = m_targetPose;
m_mustUpdateOffset = false;
} else {
transformedPose = m_odomOffset * m_odomPose;
}
//
// publish the transform over tf first
//
m_odomTransformMsg.header.stamp = stamp;
tf::transformTFToMsg(transformedPose, m_odomTransformMsg.transform);
m_transformBroadcaster.sendTransform(m_odomTransformMsg);
//
// Fill the Odometry msg
//
m_odom.header.stamp = stamp;
//set the velocity first (old values still valid)
m_odom.twist.twist.linear.x = (odomX - m_odom.pose.pose.position.x) / dt;
m_odom.twist.twist.linear.y = (odomY - m_odom.pose.pose.position.y) / dt;
m_odom.twist.twist.linear.z = (odomZ - m_odom.pose.pose.position.z) / dt;
// TODO: calc angular velocity!
// m_odom.twist.twist.angular.z = vth; ??
//set the position from the above calculated transform
m_odom.pose.pose.orientation = m_odomTransformMsg.transform.rotation;
m_odom.pose.pose.position.x = m_odomTransformMsg.transform.translation.x;
m_odom.pose.pose.position.y = m_odomTransformMsg.transform.translation.y;
m_odom.pose.pose.position.z = m_odomTransformMsg.transform.translation.z;
m_odomPub.publish(m_odom);
m_lastOdomTime = stamp.toSec();
}
}
ROS_INFO("naoqi_sensors stopped.");
}
void eeStateCallback(const geometry_msgs::PoseStampedConstPtr& msg) {
const geometry_msgs::PoseStamped* data = msg.get();
ee_pose.setOrigin(tf::Vector3(data->pose.position.x,data->pose.position.y,data->pose.position.z));
ee_pose.setRotation(tf::Quaternion(data->pose.orientation.x,data->pose.orientation.y,data->pose.orientation.z,data->pose.orientation.w));
isOkay = true;
}
void OdometryRemap::torsoOdomCallback(const nao_msgs::TorsoOdometryConstPtr& msgOdom, const nao_msgs::TorsoIMUConstPtr& msgIMU){
if (m_paused){
ROS_DEBUG("Skipping odometry callback, paused");
return;
}
double odomTime = (msgOdom->header.stamp).toSec();
ROS_DEBUG("Received [%f %f %f %f (%f/%f) (%f/%f) %f]", odomTime, msgOdom->x, msgOdom->y, msgOdom->z, msgOdom->wx, msgIMU->angleX, msgOdom->wy, msgIMU->angleY, msgOdom->wz);
double dt = (odomTime - m_lastOdomTime);
tf::Quaternion q;
// roll and pitch from IMU, yaw from odometry:
if (m_useIMUAngles)
q.setRPY(msgIMU->angleX, msgIMU->angleY, msgOdom->wz);
else
q.setRPY(msgOdom->wx, msgOdom->wy, msgOdom->wz);
m_odomPose.setOrigin(tf::Vector3(msgOdom->x, msgOdom->y, msgOdom->z));
m_odomPose.setRotation(q);
// apply offset transformation:
tf::Pose transformedPose;
if(m_mustUpdateOffset) {
if(!m_isInitialized) {
if(m_initializeFromIMU) {
// Initialization from IMU: Take x, y, z, yaw from odometry, roll and pitch from IMU
m_targetPose.setOrigin(m_odomPose.getOrigin());
m_targetPose.setRotation(tf::createQuaternionFromRPY(msgIMU->angleX, msgIMU->angleY, msgOdom->wz));
} else if(m_initializeFromOdometry) {
m_targetPose.setOrigin(m_odomPose.getOrigin());
m_targetPose.setRotation(tf::createQuaternionFromRPY(msgOdom->wx, msgOdom->wy, msgOdom->wz));
}
m_isInitialized = true;
} else {
// Overwrite target pitch and roll angles with IMU data
const double target_yaw = tf::getYaw(m_targetPose.getRotation());
if(m_initializeFromIMU) {
m_targetPose.setRotation(tf::createQuaternionFromRPY(msgIMU->angleX, msgIMU->angleY, target_yaw));
} else if(m_initializeFromOdometry){
m_targetPose.setRotation(tf::createQuaternionFromRPY(msgOdom->wx, msgOdom->wy, target_yaw));
}
}
m_odomOffset = m_targetPose * m_odomPose.inverse();
transformedPose = m_targetPose;
m_mustUpdateOffset = false;
} else {
transformedPose = m_odomOffset * m_odomPose;
}
// publish the transform over tf first:
m_odomTransformMsg.header.stamp = msgOdom->header.stamp;
tf::transformTFToMsg(transformedPose, m_odomTransformMsg.transform);
m_transformBroadcaster.sendTransform(m_odomTransformMsg);
//next, publish the actual odometry message:
m_odom.header.stamp = msgOdom->header.stamp;
m_odom.header.frame_id = m_odomFrameId;
//set the velocity first (old values still valid)
m_odom.twist.twist.linear.x = (msgOdom->x - m_odom.pose.pose.position.x) / dt;
m_odom.twist.twist.linear.y = (msgOdom->y - m_odom.pose.pose.position.y) / dt;
m_odom.twist.twist.linear.z = (msgOdom->z - m_odom.pose.pose.position.z) / dt;
// TODO: calc angular velocity!
// m_odom.twist.twist.angular.z = vth; ??
//set the position from the above calculated transform
m_odom.pose.pose.orientation = m_odomTransformMsg.transform.rotation;
m_odom.pose.pose.position.x = m_odomTransformMsg.transform.translation.x;
m_odom.pose.pose.position.y = m_odomTransformMsg.transform.translation.y;
m_odom.pose.pose.position.z = m_odomTransformMsg.transform.translation.z;
//publish the message
m_odomPub.publish(m_odom);
m_lastOdomTime = odomTime;
// TODO: not used
// m_lastWx = msgOdom->wx;
// m_lastWy = msgOdom->wy;
// m_lastWz = msgOdom->wz;
}