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);
    }
  }
Esempio n. 2
0
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;
  }