static tf::Quaternion
carmen_quaternion_to_tf_quaternion(carmen_quaternion_t carmen_quaternion)
{
tf::Quaternion tf_quat(carmen_quaternion.q1, carmen_quaternion.q2, carmen_quaternion.q3, carmen_quaternion.q0);
return tf_quat;
}
static tf::Quaternion
carmen_rotation_to_tf_quaternion(carmen_orientation_3D_t carmen_orientation)
{
tf::Quaternion tf_quat(carmen_orientation.yaw, carmen_orientation.pitch, carmen_orientation.roll);
return tf_quat;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "odom_publisher") ;
ros::NodeHandle n1 ;
ros::NodeHandle n2 ;
ros::Rate loop_rate(10) ;
ros::ServiceClient client ;
ros::Publisher pub = n1.advertise<nav_msgs::Odometry>("odom", 50) ;
std::string modelName = (std::string)"pioneer" ;
std::string relativeEntityName = (std::string)"world" ;
gazebo_msgs::GetModelState getModelState ;
geometry_msgs::Point pp ;
geometry_msgs::Quaternion qq ;
geometry_msgs::Twist current_Twist ;
while (ros::ok())
{
client = n2.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state") ;
getModelState.request.model_name = modelName ;
getModelState.request.relative_entity_name = relativeEntityName ;
client.call(getModelState) ;
pp = getModelState.response.pose.position ;
qq = getModelState.response.pose.orientation ;
tf::Quaternion tf_quat(qq.x, qq.y, qq.z, qq.w) ;
current_Twist = getModelState.response.twist ;
geometry_msgs::TransformStamped odom_trans;
odom_trans.transform.translation.x = pp.x ;
odom_trans.transform.translation.y = pp.y ;
odom_trans.transform.translation.z = pp.z ;
geometry_msgs::Quaternion geo_Quat ;
tf::quaternionTFToMsg(tf_quat, geo_Quat) ;
odom_trans.transform.rotation = geo_Quat ;
odom_trans.header.stamp = ros::Time::now() ;
odom_trans.header.frame_id = "odom" ;
odom_trans.child_frame_id = "base_footprint" ;
nav_msgs::Odometry odom ;
odom.header.stamp = odom_trans.header.stamp ;
odom.header.frame_id = "odom" ;
//set the position
odom.pose.pose.position.x = odom_trans.transform.translation.x ;
odom.pose.pose.position.y = odom_trans.transform.translation.y ;
odom.pose.pose.position.z = odom_trans.transform.translation.z ;
odom.pose.pose.orientation = geo_Quat ;
//set the velocity
odom.child_frame_id = "base_footprint";
odom.twist.twist.linear.x = current_Twist.linear.x ;
odom.twist.twist.linear.y = current_Twist.linear.y ;
odom.twist.twist.linear.z = current_Twist.linear.z ;
odom.twist.twist.angular.x= current_Twist.angular.x ;
odom.twist.twist.angular.y= current_Twist.angular.y ;
odom.twist.twist.angular.z= current_Twist.angular.z ;
//publish the message
pub.publish(odom);
ros::spinOnce();
loop_rate.sleep() ;
}
return 0 ;
}
