<commit_before>/*
 *  Copyright (c) 2015, Nagoya University
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither the name of Autoware nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 *  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 *  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 *  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <iostream>
#include <vector>
#include <string>
#include <algorithm>

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_listener.h>
#include <nav_msgs/OccupancyGrid.h>

namespace
{
constexpr auto OGM_FRAME = "/map";
constexpr int OCCUPIED_MAX = 8;
constexpr int OCCUPIED_MIN = -8;
constexpr int OCCUPIED_INCREMENT = 2;
constexpr int FREE_INCREMENT = 1;
std::string g_sensor_frame = "/velodyne";  // sensor which publihes lasescan message
std::string g_scan_topic = "/scan";        // laser scan topic
double g_resolution = 0.1;                 // [m]
int g_scan_size_x = 1000;                  // actual scanning size
int g_scan_size_y = 1000;
int g_map_size_x = 500;  // publishing occupancy grid map size
int g_map_size_y = 500;

struct Grid
{
  int index;
  int index_x, index_y;
  double x, y;
  double weight;
  double range;

  void calcCoordinate();
  void calcRange();
  int calcGlobalIndex(const tf::StampedTransform& transform) const;
};

struct Cost
{
  int occupied = 0;
  int free     = 0;

  void accumulateCost(int occ, int free);
};

ros::Publisher g_map_pub;
tf::TransformListener* g_tf_listenerp;

double calcYawFromQuaternion(const tf::Quaternion& q)
{
  tf::Matrix3x3 m(q);
  double roll, pitch, yaw;
  m.getRPY(roll, pitch, yaw);

  return yaw;
}

// Accumulate cost value
void Cost::accumulateCost(int occupied_inc, int free_inc)
{
  occupied += occupied_inc - free_inc;
  free     += free_inc;

  if (occupied > OCCUPIED_MAX)
    occupied = OCCUPIED_MAX;

  if (occupied < OCCUPIED_MIN)
    occupied = OCCUPIED_MIN;
}

// Calcurate grid's coordinate in sensor frame
void Grid::calcCoordinate()
{
  // index coordinate
  index_x = index % g_scan_size_x;
  index_y = (index - x) / g_scan_size_x;

  // actual coordinate
  x = (index_x - g_scan_size_x / 2.0) * g_resolution;
  y = (index_y - g_scan_size_y / 2.0) * g_resolution;
}

// Calculate a range from sensor in a certain grid
void Grid::calcRange()
{
  double distance_x = g_resolution * fabs(g_scan_size_x / 2 - index_x);
  double distance_y = g_resolution * fabs(g_scan_size_y / 2 - index_y);

  range = sqrt(distance_x * distance_x + distance_y * distance_y);
}

// Change local index into global index
int Grid::calcGlobalIndex(const tf::StampedTransform& transform) const<commit_msg>Fix calculation of index<commit_after>/*
 *  Copyright (c) 2015, Nagoya University
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither the name of Autoware nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 *  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 *  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 *  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <iostream>
#include <vector>
#include <string>
#include <algorithm>

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_listener.h>
#include <nav_msgs/OccupancyGrid.h>

namespace
{
constexpr auto OGM_FRAME = "/map";
constexpr int OCCUPIED_MAX = 8;
constexpr int OCCUPIED_MIN = -8;
constexpr int OCCUPIED_INCREMENT = 2;
constexpr int FREE_INCREMENT = 1;
std::string g_sensor_frame = "/velodyne";  // sensor which publihes lasescan message
std::string g_scan_topic = "/scan";        // laser scan topic
double g_resolution = 0.1;                 // [m]
int g_scan_size_x = 1000;                  // actual scanning size
int g_scan_size_y = 1000;
int g_map_size_x = 500;  // publishing occupancy grid map size
int g_map_size_y = 500;

struct Grid
{
  int index;
  int index_x, index_y;
  double x, y;
  double weight;
  double range;

  void calcCoordinate();
  void calcRange();
  int calcGlobalIndex(const tf::StampedTransform& transform) const;
};

struct Cost
{
  int occupied = 0;
  int free     = 0;

  void accumulateCost(int occ, int free);
};

ros::Publisher g_map_pub;
tf::TransformListener* g_tf_listenerp;

double calcYawFromQuaternion(const tf::Quaternion& q)
{
  tf::Matrix3x3 m(q);
  double roll, pitch, yaw;
  m.getRPY(roll, pitch, yaw);

  return yaw;
}

// Accumulate cost value
void Cost::accumulateCost(int occupied_inc, int free_inc)
{
  occupied += occupied_inc - free_inc;
  free     += free_inc;

  if (occupied > OCCUPIED_MAX)
    occupied = OCCUPIED_MAX;

  if (occupied < OCCUPIED_MIN)
    occupied = OCCUPIED_MIN;
}

// Calcurate grid's coordinate in sensor frame
void Grid::calcCoordinate()
{
  // index coordinate
  index_x = index % g_scan_size_x;
  index_y = (index - index_x) / g_scan_size_x;

  // actual coordinate
  x = (index_x - g_scan_size_x / 2.0) * g_resolution;
  y = (index_y - g_scan_size_y / 2.0) * g_resolution;
}

// Calculate a range from sensor in a certain grid
void Grid::calcRange()
{
  double distance_x = g_resolution * fabs(g_scan_size_x / 2 - index_x);
  double distance_y = g_resolution * fabs(g_scan_size_y / 2 - index_y);

  range = sqrt(distance_x * distance_x + distance_y * distance_y);
}

// Change local index into global index
int Grid::calcGlobalIndex(const tf::StampedTransform& transform) const
{'fraction_non_alphanumeric': 0.08279034112686853, 'fraction_numerical': 0.008687875303436822, 'mean_word_length': 3.941919191919192, 'pattern_counts': {'":': 0, '<': 21, '<?xml version=': 0, '>': 21, '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': '7065171', 'n_tokens_mistral': 2586, 'n_tokens_neox': 2403, 'n_words': 884}