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