void NavSatTransform::setTransformOdometry(const nav_msgs::OdometryConstPtr& msg)
{
tf2::fromMsg(msg->pose.pose, transformWorldPose_);
worldFrameId_ = msg->header.frame_id;
baseLinkFrameId_ = msg->child_frame_id;
hasTransformOdom_ = true;
ROS_INFO_STREAM("Initial odometry position is (" << std::fixed <<
transformWorldPose_.getOrigin().getX() << ", " <<
transformWorldPose_.getOrigin().getY() << ", " <<
transformWorldPose_.getOrigin().getZ() << ")");
// Users can optionally use the (potentially fused) heading from
// the odometry source, which may have multiple fused sources of
// heading data, and so would act as a better heading for the
// UTM->world_frame transform.
if(!transformGood_ && useOdometryYaw_ && !useManualDatum_)
{
sensor_msgs::Imu *imu = new sensor_msgs::Imu();
imu->orientation = msg->pose.pose.orientation;
imu->header.frame_id = msg->child_frame_id;
sensor_msgs::ImuConstPtr imuPtr(imu);
imuCallback(imuPtr);
}
}
void rawCallback(const ric_robot::ric_raw::ConstPtr& msg) {
imuCallback(msg);
int32_t left_ticks=msg->encoders[0]; // left_ticks;
int32_t right_ticks=msg->encoders[1]; // right_ticks;
encodersCallback( left_ticks, right_ticks);
float left_urf=msg->urf[0];// left_urf;
float rear_urf=msg->urf[1];// rear_urf;
float right_urf=msg->urf[2];// right_urf;
urfCallback(left_urf, rear_urf, right_urf);
}
void multiCallback(topic_tools::ShapeShifter const &input) {
if (input.getDataType() == "nav_msgs/Odometry") {
nav_msgs::Odometry::ConstPtr odom = input.instantiate<nav_msgs::Odometry>();
odomCallback(*odom);
return;
}
if (input.getDataType() == "geometry_msgs/PoseStamped") {
geometry_msgs::PoseStamped::ConstPtr pose = input.instantiate<geometry_msgs::PoseStamped>();
poseCallback(*pose);
return;
}
if (input.getDataType() == "sensor_msgs/Imu") {
sensor_msgs::Imu::ConstPtr imu = input.instantiate<sensor_msgs::Imu>();
imuCallback(*imu);
return;
}
ROS_ERROR_THROTTLE(1.0, "message_to_tf received a %s message. Supported message types: nav_msgs/Odometry geometry_msgs/PoseStamped sensor_msgs/Imu", input.getDataType().c_str());
}
bool NavSatTransform::datumCallback(robot_localization::SetDatum::Request& request,
robot_localization::SetDatum::Response&)
{
transformGood_ = false;
sensor_msgs::NavSatFix *fix = new sensor_msgs::NavSatFix();
fix->latitude = request.geo_pose.position.latitude;
fix->longitude = request.geo_pose.position.longitude;
fix->altitude = request.geo_pose.position.altitude;
fix->header.stamp = ros::Time::now();
fix->position_covariance[0] = 0.1;
fix->position_covariance[4] = 0.1;
fix->position_covariance[8] = 0.1;
fix->position_covariance_type = sensor_msgs::NavSatStatus::STATUS_FIX;
sensor_msgs::NavSatFixConstPtr fixPtr(fix);
setTransformGps(fixPtr);
nav_msgs::Odometry *odom = new nav_msgs::Odometry();
odom->pose.pose.orientation.x = 0;
odom->pose.pose.orientation.y = 0;
odom->pose.pose.orientation.z = 0;
odom->pose.pose.orientation.w = 1;
odom->pose.pose.position.x = 0;
odom->pose.pose.position.y = 0;
odom->pose.pose.position.z = 0;
odom->header.frame_id = worldFrameId_;
odom->child_frame_id = baseLinkFrameId_;
nav_msgs::OdometryConstPtr odomPtr(odom);
setTransformOdometry(odomPtr);
sensor_msgs::Imu *imu = new sensor_msgs::Imu();
imu->orientation = request.geo_pose.orientation;
imu->header.frame_id = baseLinkFrameId_;
sensor_msgs::ImuConstPtr imuPtr(imu);
imuCallback(imuPtr);
return true;
}
