static inline geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw) { geometry_msgs::Quaternion q; q = setRPY(0.0, 0.0, yaw); geometry_msgs::Quaternion q_msg; quaternionTFToMsg(q, q_msg); return q_msg; }
void State::setOrientation(const geometry_msgs::Quaternion& q) { double roll, pitch, yaw; tf::Quaternion q_tf; tf::quaternionMsgToTF(q, q_tf); tf::Matrix3x3(q_tf).getRPY(roll, pitch, yaw); setRPY(roll, pitch, yaw); }
void leatherman::poseVectorToPoseMsg(const std::vector<double> &pose, geometry_msgs::Pose &pose_msg) { if(pose.size() <6) ROS_ERROR("Pose must have 6 elements. Cannot convert pose to pose message."); else { tf::Quaternion btpose; pose_msg.position.x = pose[0]; pose_msg.position.y = pose[1]; pose_msg.position.z = pose[2]; btpose = setRPY(pose[3], pose[4], pose[5]); tf::quaternionTFToMsg(btpose, pose_msg.orientation); } }
bool Tracks::getRPY() { setRPY(); int start; std::string output; getData(4); usleep(_delay); while(ros::ok()) { readData(output,1); if(output[0]==0x00) break; } this->readData(output, 5 + 5*sensor_used); output.insert(output.begin(),0x00); std::string buff; float value; buff = output.substr(0,4); value = dataToFloat32(buff); std::string payload = output.substr(0,sensor_used*5+4); unsigned short crc = (unsigned short)output[sensor_used*5+4]<<8; crc +=output[sensor_used*5+5]; unsigned short crc_cal = getCRC16(payload); if(crc!=crc_cal) return false; printf("crc: %x %x ",crc,crc_cal); //int cast = reinterpret_cast<int>(value); printf("RPY : %x %x %x %x \n",(unsigned char)buff[0],(unsigned char)buff[1],(unsigned char)buff[2],(unsigned char)buff[3]); int j=roll; for(start = 4;start<(sensor_used*5+1);start+=5) { buff = output.substr(start,5); value = dataToFloat32(buff); printf("%x %x %x %x %x\t\t",(unsigned char)buff[0],(unsigned char)buff[1],(unsigned char)buff[2],(unsigned char)buff[3],(unsigned char)buff[4]); std::cout<<value<<"\n"; _data.data[j++]=value; } std::cout<<std::endl; return true; }
void leatherman::rpyToQuatMsg(double r, double p, double y, geometry_msgs::Quaternion &q) { tf::Quaternion btpose; btpose = setRPY(r, p, y); tf::quaternionTFToMsg(btpose, q); }
OrientationMatrix::OrientationMatrix(double roll, double pitch, double yaw) { setRPY(roll, pitch, yaw); }