diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc index 2d93f59..da5e925 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc @@ -26,6 +26,9 @@ #include #include +#include "tf/transform_datatypes.h"//转换函数头文件 +#include //里程计信息格式 + #include #include @@ -1222,11 +1225,15 @@ class ROSWrapperNodelet : public nodelet::Nodelet { mesh_msg_.header.frame_id = base_frame_id_; mesh_msg_.header.stamp = ros::Time::now(); mesh_msg_.type = visualization_msgs::Marker::MESH_RESOURCE; + geometry_msgs::Quaternion q; + double Pie = 3.1416; + q = tf::createQuaternionMsgFromRollPitchYaw(Pie/2, 0.0, Pie/2); + // fill orientation - mesh_msg_.pose.orientation.x = 1; - mesh_msg_.pose.orientation.y = 1; - mesh_msg_.pose.orientation.z = 1; - mesh_msg_.pose.orientation.w = 1; + mesh_msg_.pose.orientation.x = q.x; + mesh_msg_.pose.orientation.y = q.y; + mesh_msg_.pose.orientation.z = q.z; + mesh_msg_.pose.orientation.w = q.w; // fill position mesh_msg_.pose.position.x = 0; @@ -1239,7 +1246,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet { mesh_msg_.scale.z = 0.003; mesh_msg_.action = visualization_msgs::Marker::ADD; - mesh_msg_.color.a = 1.0; // Don't forget to set the alpha! + mesh_msg_.color.a = 0.5; // Don't forget to set the alpha! mesh_msg_.color.r = 1.0; mesh_msg_.color.g = 1.0; mesh_msg_.color.b = 1.0;