<commit_before>#include <ros/ros.h>
#include <ros/subscriber.h>
#include <ros/publisher.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include "GraphSearch.hpp"
#include "igvcsearchproblem.h"

ros::Publisher disp_path_pub;

IGVCSearchProblem search_problem;

void map_callback(const sensor_msgs::PointCloud2ConstPtr &msg)
{

}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "pathplanner");

    ros::NodeHandle nh;

    nh.subscribe("/map", 1, map_callback);

    disp_path_pub = nh.advertise<nav_msgs::Path>("/path_display", 1);

    ros::spin();

    return 0;
}<commit_msg>Some more work to ROS path planning.<commit_after>#include <ros/ros.h>
#include <ros/subscriber.h>
#include <ros/publisher.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include "GraphSearch.hpp"
#include "igvcsearchproblem.h"
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <tf/transform_datatypes.h>
#include <mutex>
#include <pcl_conversions/pcl_conversions.h>

using namespace std;

ros::Publisher disp_path_pub;

IGVCSearchProblem search_problem;

mutex planning_mutex;

void map_callback(const sensor_msgs::PointCloud2ConstPtr &msg)
{
    lock_guard<mutex> lock(planning_mutex);
    pcl::fromROSMsg(*msg, *search_problem.Map);
}

void position_callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
{
    lock_guard<mutex> lock(planning_mutex);
    search_problem.Start.x = msg->pose.pose.position.x;
    search_problem.Start.y = msg->pose.pose.position.y;
    tf::Quaternion q{msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w};
    tf::Matrix3x3 m{q};
    double roll, pitch, yaw;
    m.getRPY(roll, pitch, yaw);
    search_problem.Start.theta = yaw;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "pathplanner");

    ros::NodeHandle nh;

    nh.subscribe("/map", 1, map_callback);

    nh.subscribe("robot_pose_ekf/odom_combined", 1, position_callback);

    disp_path_pub = nh.advertise<nav_msgs::Path>("/path_display", 1);

    ros::Rate rate(3);
    while(ros::ok())
    {
        ros::spinOnce();

        planning_mutex.lock();
        // TODO replan if needed.
        planning_mutex.unlock();

        rate.sleep();
    }

    return 0;
}
{'fraction_non_alphanumeric': 0.14543926019335857, 'fraction_numerical': 0.0058848255569567045, 'mean_word_length': 4.707434052757794, 'pattern_counts': {'":': 0, '<': 23, '<?xml version=': 0, '>': 29, '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': '2987075', 'n_tokens_mistral': 917, 'n_tokens_neox': 863, 'n_words': 148}