plugins/examples/org.eclipse.papyrus.robotics.ros2.examples/models/dummy_robot/src/dummy_laserCompdef/Dummy_laser_impl.cpp

// --------------------------------------------------------
// Copyright (c)
//
// contributions by author
//                  author@somewhere.net
// maintained by    maintainer
//                  maintainer@somewhere.net

// --------------------------------------------------------
// Code generated by Papyrus C++
// --------------------------------------------------------

#define dummy_laserCompdef_Dummy_laser_impl_BODY

/************************************************************
 Dummy_laser_impl class body
 ************************************************************/

// include associated header file
#include "dummy_laserCompdef/Dummy_laser_impl.h"

// Derived includes directives
#include "rclcpp/rclcpp.hpp"

#include <math.h>
#define DEG2RAD M_PI / 180.0

namespace dummy_laserCompdef {

// static attributes (if any)

/**
 * 
 * @param options 
 */
Dummy_laser_impl::Dummy_laser_impl(rclcpp::NodeOptions /*in*/options) :
    Dummy_laser(options) {
}

/**
 * 
 */
void Dummy_laser_impl::activateFct() {
  msg.header.frame_id = "single_rrbot_hokuyo_link";

  double angle_resolution = 2500;
  double start_angle = -450000;
  double stop_angle = 2250000;
  double scan_frequency = 2500;

  double angle_range = stop_angle - start_angle;
  double num_values = angle_range / angle_resolution;
  if (static_cast<int>(angle_range) % static_cast<int>(angle_resolution) == 0) {
      // Include endpoint
      ++num_values;
  }
  msg.ranges.resize(static_cast<int>(num_values));

  msg.time_increment =
      static_cast<float>((angle_resolution / 10000.0) / 360.0 / (scan_frequency / 100.0));
  msg.angle_increment = static_cast<float>(angle_resolution / 10000.0 * DEG2RAD);
  msg.angle_min = static_cast<float>(start_angle / 10000.0 * DEG2RAD - M_PI / 2);
  msg.angle_max = static_cast<float>(stop_angle / 10000.0 * DEG2RAD - M_PI / 2);
  msg.scan_time = static_cast<float>(100.0 / scan_frequency);
  msg.range_min = 0.0f;
  msg.range_max = 10.0f;

  RCLCPP_INFO(get_logger(), "angle inc:\t%f", msg.angle_increment);
  RCLCPP_INFO(get_logger(), "scan size:\t%zu", msg.ranges.size());
  RCLCPP_INFO(get_logger(), "scan time increment: \t%f", msg.time_increment);
}

/**
 * 
 */
void Dummy_laser_impl::periodicFct() {
  auto amplitude = 1;
  counter += 0.1;
  auto distance = static_cast<float>(std::abs(amplitude * std::sin(counter)));

  for (size_t i = 0; i < msg.ranges.size(); ++i) {
      msg.ranges[i] = distance;
  }

  scan_pub_->publish(msg);}
} // of namespace dummy_laserCompdef

/************************************************************
 End of Dummy_laser_impl class body
 ************************************************************/