本文整理汇总了C++中tf::Quaternion::getW方法的典型用法代码示例。如果您正苦于以下问题：C++ Quaternion::getW方法的具体用法？C++ Quaternion::getW怎么用？C++ Quaternion::getW使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类tf::Quaternion的用法示例。
在下文中一共展示了Quaternion::getW方法的4个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: while
//.........这里部分代码省略.........
index = marker_counter_;
markers_[index].marker_id = current_marker_id;
existing = true;
ROS_DEBUG_STREAM("New marker with ID: " << current_marker_id << " found");
}
// Change visibility flag of new marker
for(size_t j = 0;j < marker_counter_; j++)
{
for(size_t k = 0;k < temp_markers.size(); k++)
{
if(markers_[j].marker_id == temp_markers[k].id)
markers_[j].visible = true;
}
}
//------------------------------------------------------
// For existing marker do
//------------------------------------------------------
if((index < marker_counter_) && (first_marker_detected_ == true))
{
markers_[index].current_camera_tf = arucoMarker2Tf(temp_markers[i]);
markers_[index].current_camera_tf = markers_[index].current_camera_tf.inverse();
const tf::Vector3 marker_origin = markers_[index].current_camera_tf.getOrigin();
markers_[index].current_camera_pose.position.x = marker_origin.getX();
markers_[index].current_camera_pose.position.y = marker_origin.getY();
markers_[index].current_camera_pose.position.z = marker_origin.getZ();
const tf::Quaternion marker_quaternion = markers_[index].current_camera_tf.getRotation();
markers_[index].current_camera_pose.orientation.x = marker_quaternion.getX();
markers_[index].current_camera_pose.orientation.y = marker_quaternion.getY();
markers_[index].current_camera_pose.orientation.z = marker_quaternion.getZ();
markers_[index].current_camera_pose.orientation.w = marker_quaternion.getW();
}
//------------------------------------------------------
// For new marker do
//------------------------------------------------------
if((index == marker_counter_) && (first_marker_detected_ == true))
{
markers_[index].current_camera_tf=arucoMarker2Tf(temp_markers[i]);
tf::Vector3 marker_origin = markers_[index].current_camera_tf.getOrigin();
markers_[index].current_camera_pose.position.x = marker_origin.getX();
markers_[index].current_camera_pose.position.y = marker_origin.getY();
markers_[index].current_camera_pose.position.z = marker_origin.getZ();
tf::Quaternion marker_quaternion = markers_[index].current_camera_tf.getRotation();
markers_[index].current_camera_pose.orientation.x = marker_quaternion.getX();
markers_[index].current_camera_pose.orientation.y = marker_quaternion.getY();
markers_[index].current_camera_pose.orientation.z = marker_quaternion.getZ();
markers_[index].current_camera_pose.orientation.w = marker_quaternion.getW();
// Naming - TFs
std::stringstream camera_tf_id;
std::stringstream camera_tf_id_old;
std::stringstream marker_tf_id_old;
camera_tf_id << "camera_" << index;
// Flag to keep info if any_known marker_visible in actual image
bool any_known_marker_visible = false;
// Array ID of markers, which position of new marker is calculated
int last_marker_id;示例2: MakeInteractiveMarker
void InteractiveMarkerArrow::MakeInteractiveMarker(std::string intMarkerName, tf::Quaternion qx_control, tf::Quaternion qy_control, tf::Quaternion qz_control)
{
visualization_msgs::InteractiveMarker int_marker;
int_marker.header.frame_id = "world";
int_marker.scale = 0.1;
int_marker.name = intMarkerName;
geometry_msgs::Pose pose;
tfSrv->get("world", intMarkerName + "_control", pose);
int_marker.pose = pose;
InteractiveMarkerArrow::MakeControl(int_marker);
int_marker.controls[0].interaction_mode = 7;
visualization_msgs::InteractiveMarkerControl control;
control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
tf::Transform tr;
tfSrv->get("world", "viceGripRotation", tr);
qx_control = tr.getRotation() * qx_control;
qy_control = tr.getRotation() * qy_control;
qz_control = tr.getRotation() * qz_control;
control.orientation.w = qx_control.getW();
control.orientation.x = qx_control.getX();
control.orientation.y = qx_control.getY();
control.orientation.z = qx_control.getZ();
control.name = "rotate_x";
control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back(control);
control.name = "move_x";
control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(control);
control.orientation.w = qz_control.getW();
control.orientation.x = qz_control.getX();
control.orientation.y = qz_control.getY();
control.orientation.z = qz_control.getZ();
control.name = "rotate_z";
control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back(control);
control.name = "move_z";
control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(control);
control.orientation.w = qy_control.getW();
control.orientation.x = qy_control.getX();
control.orientation.y = qy_control.getY();
control.orientation.z = qy_control.getZ();
control.name = "rotate_y";
control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back(control);
control.name = "move_y";
control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(control);
intMarkerSrv->insert(int_marker);
intMarkerSrv->setCallback(int_marker.name, _processFeedBackTemp(boost::bind(&InteractiveMarkerArrow::ProcessFeedback, this, _1)));
intMarkerSrv->applyChanges();
}示例3: leapmotionCallback
void LeapMotionListener::leapmotionCallback(const leap_motion::leapros2::ConstPtr& dataHand)
{
dataHand_=(*dataHand);
ROS_INFO("ORIENTATION OF THE HAND SET TO \n X: %f\n Y: %f\n Z: %f\n ",dataHand_.ypr.x,dataHand_.ypr.y,dataHand_.ypr.z);
ROS_INFO("DIRECTION OF THE HAND SET TO \n X: %f\n Y: %f\n Z: %f\n ",dataHand_.direction.x,dataHand_.direction.y,dataHand_.direction.z);
ROS_INFO("NORMAL OF THE HAND SET TO \n X: %f\n Y: %f\n Z: %f\n ",dataHand_.normal.x,dataHand_.normal.y,dataHand_.normal.z);
//ROS_INFO("INSIDE CALLBACK");
//We create the values of reference for the first postion of our hand
if (FIRST_VALUE)
{
dataLastHand_.palmpos.x=dataHand_.palmpos.x;
dataLastHand_.palmpos.y=dataHand_.palmpos.y;
dataLastHand_.palmpos.z=dataHand_.palmpos.z;
FIRST_VALUE=0;
Updifferencex=dataLastHand_.palmpos.x+10;
Downdifferencex=dataLastHand_.palmpos.x-10;
Updifferencez=dataLastHand_.palmpos.z+10;
Downdifferencez=dataLastHand_.palmpos.z-20;
Updifferencey=dataLastHand_.palmpos.y+20;
Downdifferencey=dataLastHand_.palmpos.y-20;
//ROS_INFO("ORIGINAL POSITION OF THE HAND SET TO \n X: %f\n Y: %f\n Z: %f\n ",trajectory_hand.at(i).palmpos.x,trajectory_hand.at(i).palmpos.y,trajectory_hand.at(i).palmpos.z);
//sleep(2);
}
else
{
//We get the distance between the finger and transform it into gripper distance
rot7=DtA+DtAx*dataHand_.finger_distance;
rot8=DtA+DtAx*dataHand_.finger_distance;
joint_msg_leap=jointstate_;
joint_msg_leap.position[7] = -rot8;
joint_msg_leap.position[6] = rot7;
if ((dataHand_.palmpos.x<Downdifferencex)||(dataHand_.palmpos.x>Updifferencex)||(dataHand_.palmpos.y<Downdifferencey)||(dataHand_.palmpos.y>Updifferencey)||(dataHand_.palmpos.z<Downdifferencez)||(dataHand_.palmpos.z>Updifferencez))
{
//q.setRPY(0,0,M_PI/2);//Fixed Position for testing..with this pose the robot is oriented to the ground
q.setRPY(dataHand_.ypr.x,dataHand_.ypr.y,dataHand_.ypr.z);
pose.orientation.x = q.getAxis().getX();//cambiado aposta getZ()
pose.orientation.y = q.getAxis().getY();
pose.orientation.z = q.getAxis().getZ();//cambiado aposta getX()
pose.orientation.w = q.getW();
//pose.orientation.w = ;
//pose.orientation.z=1;
//pose.orientation.y=0;
//pose.orientation.x=0;
//We need to send the correct axis to the robot. Currently they are rotated and x is z
//ROS_INFO("VALUES OF THE QUATERNION SET TO \n X: %f\n Y: %f\n Z: %f W: %f\n",pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w);
pose.position.y +=(dataHand_.palmpos.x-dataLastHand_.palmpos.x) ;
pose.position.z +=(dataHand_.palmpos.y-dataLastHand_.palmpos.y);
if(pose.position.z>Uplimitez)
pose.position.z=Uplimitez;
pose.position.x +=(dataHand_.palmpos.z-dataLastHand_.palmpos.z);
//Here we instantiate an autogenerated service class
srv.request.target = pose ;
if (pclient->call(srv))
{
//ROS_INFO("Ret: %d", (int)srv.response.ret);
dataLastHand_.palmpos.x=dataHand_.palmpos.x;
dataLastHand_.palmpos.y=dataHand_.palmpos.y;
dataLastHand_.palmpos.z=dataHand_.palmpos.z;
// Both limits for x,y,z to avoid small changes
Updifferencex=dataLastHand_.palmpos.x+10;//
Downdifferencex=dataLastHand_.palmpos.x-10;
Updifferencez=dataLastHand_.palmpos.z+10;
Downdifferencez=dataLastHand_.palmpos.z-20;
Updifferencey=dataLastHand_.palmpos.y+20;
Downdifferencey=dataLastHand_.palmpos.y-20;
old_pose=pose;
ROS_INFO("response %f\n",srv.response.ret);
}
else
{
ROS_ERROR("Position out of range");
pose=old_pose;
}
}
//We get the aswer of the service and publish it into the joint_msg_leap message
//joint_msg_leap.position[0] = srv.response.joint1;
//joint_msg_leap.position[1] = srv.response.joint2;
//joint_msg_leap.position[2] = srv.response.joint3;
//joint_msg_leap.position[3] = srv.response.joint4;
//joint_msg_leap.position[4] = srv.response.joint5;
//joint_msg_leap.position[5] = srv.response.joint6;
//robo_pub.publish(joint_msg_leap);
}
//ROS_INFO("END EFFECTOR POSITION \n X: %f\n Y: %f\n Z: %f\n", pose.position.x,pose.position.y,pose.position.z);
//ROS_INFO("ORIENTATION OF THE HAND SET TO \n X: %f\n Y: %f\n Z: %f\n ",q.getAxis().getX(),q.getAxis().getY(),q.getAxis().getZ());
}示例4: complete_message_callback
//.........这里部分代码省略.........
R_fc = first_R;
T_fc = first_T;
fc_found = true;
}
//if a solution was found will publish
// need to convert to pose message so use
if (fc_found)
{
// converting the rotation from a cv matrix to quaternion, first need it as a matrix3x3
R_fc_tf[0][0] = R_fc.at<double>(0,0);
R_fc_tf[0][1] = R_fc.at<double>(0,1);
R_fc_tf[0][2] = R_fc.at<double>(0,2);
R_fc_tf[1][0] = R_fc.at<double>(1,0);
R_fc_tf[1][1] = R_fc.at<double>(1,1);
R_fc_tf[1][2] = R_fc.at<double>(1,2);
R_fc_tf[2][0] = R_fc.at<double>(2,0);
R_fc_tf[2][1] = R_fc.at<double>(2,1);
R_fc_tf[2][2] = R_fc.at<double>(2,2);
std::cout << "Final R:\n" << R_fc << std::endl;
// converting the translation to a vector 3
T_fc_tf.setX(T_fc.at<double>(0,0));
T_fc_tf.setY(T_fc.at<double>(0,1));
T_fc_tf.setZ(T_fc.at<double>(0,2));
std::cout << "Final T :\n" << T_fc << std::endl;
// getting the rotation as a quaternion
R_fc_tf.getRotation(Q_fc_tf);
std::cout << "current orientation:" << "\n\tx:\t" << Q_fc_tf.getX()
<< "\n\ty:\t" << Q_fc_tf.getY()
<< "\n\tz:\t" << Q_fc_tf.getZ()
<< "\n\tw:\t" << Q_fc_tf.getW()
<< std::endl;
std::cout << "norm of quaternion:\t" << Q_fc_tf.length() << std::endl;
// getting the negated version of the quaternion for the check
Q_fc_tf_negated = tf::Quaternion(-Q_fc_tf.getX(),-Q_fc_tf.getY(),-Q_fc_tf.getZ(),-Q_fc_tf.getW());
std::cout << "negated orientation:" << "\n\tx:\t" << Q_fc_tf_negated.getX()
<< "\n\ty:\t" << Q_fc_tf_negated.getY()
<< "\n\tz:\t" << Q_fc_tf_negated.getZ()
<< "\n\tw:\t" << Q_fc_tf_negated.getW()
<< std::endl;
std::cout << "norm of negated quaternion:\t" << Q_fc_tf_negated.length() << std::endl;
// showing the last orientation
std::cout << "last orientation:" << "\n\tx:\t" << Q_fc_tf_last.getX()
<< "\n\ty:\t" << Q_fc_tf_last.getY()
<< "\n\tz:\t" << Q_fc_tf_last.getZ()
<< "\n\tw:\t" << Q_fc_tf_last.getW()
<< std::endl;
std::cout << "norm of last quaternion:\t" << Q_fc_tf_last.length() << std::endl;
// checking if the quaternion has flipped
Q_norm_current_diff = std::sqrt(std::pow(Q_fc_tf.getX() - Q_fc_tf_last.getX(),2.0)
+ std::pow(Q_fc_tf.getY() - Q_fc_tf_last.getY(),2.0)
+ std::pow(Q_fc_tf.getZ() - Q_fc_tf_last.getZ(),2.0)
+ std::pow(Q_fc_tf.getW() - Q_fc_tf_last.getW(),2.0));
std::cout << "current difference:\t" << Q_norm_current_diff << std::endl;