<commit_before>#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>

#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <std_msgs/Float64.h>

#include <math.h>

std::vector<std::string> landmark_frames;
std::vector<std::string> april_frames;

// Publisher that sends out an april tag that is a possible goal node?
ros::Publisher tags_pub;
// Publisher that publishes a pose for visualization purposes to the "/new_pose"
ros::Publisher new_pose_pub;
// Publisher that reinitializes the pose of the robot (relocalizes)
ros::Publisher new_initial_pose_pub;

void init(ros::NodeHandle nh)
{
  landmark_frames.push_back(std::string("/landmark_3"));
  landmark_frames.push_back(std::string("/landmark_5"));
  april_frames.push_back(std::string("/april_tag[3]"));
  april_frames.push_back(std::string("/april_tag[5]"));

  tags_pub = nh.advertise<geometry_msgs::PoseStamped>("/apriltags", 100);
  new_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/new_pose", 100);
  new_initial_pose_pub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 100);
}

bool AprilTagLocalize(tf::TransformListener &listener)
{
  static ros::Time lastPoseUpdateTime = ros::Time::now();

  if (ros::Time::now() - lastPoseUpdateTime > ros::Duration(5.1))
  {
    for (int i = 0; i < landmark_frames.size(); i++)
    {
      try{
        // if it can find a transformation between a landmark frame and april tag frame (i.e. both are in the tree)
        // watch out for tags still in tree but not seen in the camera
        if (listener.canTransform (landmark_frames[i], april_frames[i], ros::Time(0)))
        {
          tf::StampedTransform mapLandmarkTransform;
          tf::StampedTransform differenceTransform;
          tf::StampedTransform tagToBaseTransform;

          listener.lookupTransform(april_frames[i], "/base_link",
          ros::Time(0), tagToBaseTransform);
          listener.lookupTransform("map", landmark_frames[i],
          ros::Time(0), mapLandmarkTransform);
          listener.lookupTransform(landmark_frames[i], april_frames[i],
          ros::Time(0), differenceTransform);

          if (ros::Time::now() - tagToBaseTransform.stamp_ < ros::Duration(1.0))
          {

            //Create a pose based in the /map frame
            geometry_msgs::PoseWithCovariance poseWithCovariance;
            geometry_msgs::Pose pose;

            tf::Vector3 lm_vec = mapLandmarkTransform.getOrigin();
            tf::Vector3 april_vec = tagToBaseTransform.getOrigin();
            tf::Matrix3x3 rot_vec = mapLandmarkTransform.getBasis();

            // std::cout << tagToBaseTransform.getOrigin().x() <<" "<< tagToBaseTransform.getOrigin().y() <<" "<< tagToBaseTransform.getOrigin().z() <<" "<< std::endl;
            // std::cout << mapLandmarkTransform.getOrigin().x() <<" "<< mapLandmarkTransform.getOrigin().y() <<" "<< mapLandmarkTransform.getOrigin().z() <<" "<< std::endl;

            tf::Transform tf;
            tf.setOrigin(lm_vec + (april_vec * rot_vec));

            double yawMap = getYaw(mapLandmarkTransform.getRotation());
            double yawTag = getYaw(tagToBaseTransform.getRotation());

            tf.setRotation(tf::createQuaternionFromYaw(yawMap + yawTag));

            geometry_msgs::Transform msg_tf;
            tf::transformTFToMsg(tf, msg_tf);

            pose.position.x = msg_tf.translation.x;
            pose.position.y = msg_tf.translation.y;
            pose.position.z = 0;
            pose.orientation = msg_tf.rotation;

            ROS_INFO("New Robot Position:\ndx = %lf, dy = %lf, dz = %lf",
            pose.position.x, pose.position.y, pose.position.z);

            geometry_msgs::PoseWithCovarianceStamped newRobotPose;
            newRobotPose.header.frame_id = "map";
            newRobotPose.header.stamp = tagToBaseTransform.stamp_;

            poseWithCovariance.pose = pose;

            // create the covariance array (the values are based on what rviz sends)<commit_msg>Minor change<commit_after>#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>

#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <std_msgs/Float64.h>

#include <math.h>

std::vector<std::string> landmark_frames;
std::vector<std::string> april_frames;

// Publisher that sends out an april tag that is a possible goal node?
ros::Publisher tags_pub;
// Publisher that publishes a pose for visualization purposes to the "/new_pose"
ros::Publisher new_pose_pub;
// Publisher that reinitializes the pose of the robot (relocalizes)
ros::Publisher new_initial_pose_pub;

void init(ros::NodeHandle nh)
{
  landmark_frames.push_back(std::string("/landmark_3"));
  landmark_frames.push_back(std::string("/landmark_5"));
  april_frames.push_back(std::string("/april_tag[3]"));
  april_frames.push_back(std::string("/april_tag[5]"));

  tags_pub = nh.advertise<geometry_msgs::PoseStamped>("/apriltags", 100);
  new_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/new_pose", 100);
  new_initial_pose_pub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 100);
}

bool AprilTagLocalize(tf::TransformListener &listener)
{
  static ros::Time lastPoseUpdateTime = ros::Time::now();

  if (ros::Time::now() - lastPoseUpdateTime > ros::Duration(5))
  {
    for (int i = 0; i < landmark_frames.size(); i++)
    {
      try{
        // if it can find a transformation between a landmark frame and april tag frame (i.e. both are in the tree)
        // watch out for tags still in tree but not seen in the camera
        if (listener.canTransform (landmark_frames[i], april_frames[i], ros::Time(0)))
        {
          tf::StampedTransform mapLandmarkTransform;
          tf::StampedTransform differenceTransform;
          tf::StampedTransform tagToBaseTransform;

          listener.lookupTransform(april_frames[i], "/base_link",
          ros::Time(0), tagToBaseTransform);
          listener.lookupTransform("map", landmark_frames[i],
          ros::Time(0), mapLandmarkTransform);
          listener.lookupTransform(landmark_frames[i], april_frames[i],
          ros::Time(0), differenceTransform);

          if (ros::Time::now() - tagToBaseTransform.stamp_ < ros::Duration(1.0))
          {

            //Create a pose based in the /map frame
            geometry_msgs::PoseWithCovariance poseWithCovariance;
            geometry_msgs::Pose pose;

            tf::Vector3 lm_vec = mapLandmarkTransform.getOrigin();
            tf::Vector3 april_vec = tagToBaseTransform.getOrigin();
            tf::Matrix3x3 rot_vec = mapLandmarkTransform.getBasis();

            // std::cout << tagToBaseTransform.getOrigin().x() <<" "<< tagToBaseTransform.getOrigin().y() <<" "<< tagToBaseTransform.getOrigin().z() <<" "<< std::endl;
            // std::cout << mapLandmarkTransform.getOrigin().x() <<" "<< mapLandmarkTransform.getOrigin().y() <<" "<< mapLandmarkTransform.getOrigin().z() <<" "<< std::endl;

            tf::Transform tf;
            tf.setOrigin(lm_vec + (april_vec * rot_vec));

            double yawMap = getYaw(mapLandmarkTransform.getRotation());
            double yawTag = getYaw(tagToBaseTransform.getRotation());

            tf.setRotation(tf::createQuaternionFromYaw(yawMap + yawTag));

            geometry_msgs::Transform msg_tf;
            tf::transformTFToMsg(tf, msg_tf);

            pose.position.x = msg_tf.translation.x;
            pose.position.y = msg_tf.translation.y;
            pose.position.z = 0;
            pose.orientation = msg_tf.rotation;

            ROS_INFO("New Robot Position:\nx = %lf, y = %lf, z = %lf",
            pose.position.x, pose.position.y, pose.position.z);

            geometry_msgs::PoseWithCovarianceStamped newRobotPose;
            newRobotPose.header.frame_id = "map";
            newRobotPose.header.stamp = tagToBaseTransform.stamp_;

            poseWithCovariance.pose = pose;

            // create the covariance array (the values are based on what rviz sends)
{'fraction_non_alphanumeric': 0.13282803180914513, 'fraction_numerical': 0.007082504970178927, 'mean_word_length': 3.4249587685541507, 'pattern_counts': {'":': 0, '<': 89, '<?xml version=': 0, '>': 31, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1432056', 'n_tokens_mistral': 2610, 'n_tokens_neox': 2502, 'n_words': 482}