Added scan_delay

CHANGELOG.rst

@@ -2,6 +2,12 @@
 Changelog for package sicks300_2
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.3.0 (20-06-2023)
+------------------
+* Added scan_filter node.
+* Added logging info in launch file.
+* Added scan delay parameter.
+
 1.2.3 (02-05-2023)
 ------------------
 * Update on_activate and on_deactivate methods of the publishers: https://docs.ros.org/en/humble/Releases/Release-Humble-Hawksbill.html#rclcpp-lifecycle

CMakeLists.txt

@@ -33,7 +33,7 @@ add_executable(${PROJECT_NAME}
  src/common/ScannerSickS300.cpp
  src/common/SerialIO.cpp
  src/sicks300_2.cpp
- src/${PROJECT_NAME}_node.cpp
+ src/main.cpp
 )
 ament_target_dependencies(${PROJECT_NAME}
  rclcpp_lifecycle

README.md

@@ -114,6 +114,10 @@ Driver for the Sick S300 Safety laser scanners.
 
  Cycle time of the scan in seconds. Documentation says S300 scans every 40ms.
 
+* **`scan_delay`** (double, default: 0.075)
+
+ Delay between the start of the scan and the first measurement in seconds.
+
 * **`debug`** (bool, default: false)
 
  Option to toggle scanner debugging information.

include/sicks300_2/sicks300_2.hpp

@@ -50,7 +50,7 @@ class SickS3002: public rclcpp_lifecycle::LifecycleNode{
  int baud_, scan_id_;
  bool inverted_, debug_, synced_time_ready_;
  unsigned int synced_sick_stamp_;
- double scan_duration_, scan_cycle_time_, communication_timeout_;
+ double scan_duration_, scan_cycle_time_, scan_delay_, communication_timeout_;
  ScannerSickS300 scanner_;
  rclcpp::TimerBase::SharedPtr timer_;
 

params/default.yaml

@@ -5,6 +5,7 @@ laser_front:
  baud: 500000
  scan_duration: 0.025 # No info about that in SICK-docu, but 0.025 is believable and looks good in rviz
  scan_cycle_time: 0.040 # SICK-docu says S300 scans every 40ms
+ scan_delay: 0.075
  inverted: false
  scan_id: 7
  frame_id: base_laser_link

src/sicks300_2.cpp

@@ -40,103 +40,135 @@ rclcpp_CallReturn SickS3002::on_configure(const rclcpp_lifecycle::State &){
  RCLCPP_INFO(this->get_logger(), "Configuring the node...");
 
  // Declare and read parameters
- nav2_util::declare_parameter_if_not_declared(this, "port", rclcpp::ParameterValue("/dev/ttyUSB0"), 
- rcl_interfaces::msg::ParameterDescriptor()
- .set__description("USB port of the scanner"));
+ nav2_util::declare_parameter_if_not_declared(this, "port", 
+ rclcpp::ParameterValue("/dev/ttyUSB0"), 
+ rcl_interfaces::msg::ParameterDescriptor()
+ .set__description("USB port of the scanner"));
  this->get_parameter("port", port_);
  RCLCPP_INFO(this->get_logger(), "The parameter port is set to: %s", port_.c_str());
 
- nav2_util::declare_parameter_if_not_declared(this, "baud", rclcpp::ParameterValue(500000), 
- rcl_interfaces::msg::ParameterDescriptor()
- .set__description("Baudrate to communicate with the laser scanner"));
+ nav2_util::declare_parameter_if_not_declared(this, 
+ "baud", rclcpp::ParameterValue(500000), 
+ rcl_interfaces::msg::ParameterDescriptor()
+ .set__description("Baudrate to communicate with the laser scanner"));
  this->get_parameter("baud", baud_);
  RCLCPP_INFO(this->get_logger(), "The parameter baud is set to: %i", baud_);
 
- nav2_util::declare_parameter_if_not_declared(this, "scan_id", rclcpp::ParameterValue(7), 
- rcl_interfaces::msg::ParameterDescriptor()
- .set__description("Identifier of the scanner"));
+ nav2_util::declare_parameter_if_not_declared(this, 
+ "scan_id", rclcpp::ParameterValue(7), 
+ rcl_interfaces::msg::ParameterDescriptor()
+ .set__description("Identifier of the scanner"));
  this->get_parameter("scan_id", scan_id_);
  RCLCPP_INFO(this->get_logger(), "The parameter scan_id is set to: %i", scan_id_);
 
- nav2_util::declare_parameter_if_not_declared(this, "inverted", rclcpp::ParameterValue(false), 
- rcl_interfaces::msg::ParameterDescriptor()
- .set__description("Option to invert the direction of the measurements"));
+ nav2_util::declare_parameter_if_not_declared(this, 
+ "inverted", rclcpp::ParameterValue(false), 
+ rcl_interfaces::msg::ParameterDescriptor()
+ .set__description("Option to invert the direction of the measurements"));
  this->get_parameter("inverted", inverted_);
- RCLCPP_INFO(this->get_logger(), "The parameter inverted is set to: %s", inverted_ ? "true" : "false");
+ RCLCPP_INFO(this->get_logger(), 
+ "The parameter inverted is set to: %s", inverted_ ? "true" : "false");
 
- nav2_util::declare_parameter_if_not_declared(this, "scan_topic", rclcpp::ParameterValue("scan"), 
- rcl_interfaces::msg::ParameterDescriptor()
- .set__description("The topic where the laser scan will be published"));
+ nav2_util::declare_parameter_if_not_declared(this, 
+ "scan_topic", rclcpp::ParameterValue("scan"), 
+ rcl_interfaces::msg::ParameterDescriptor()
+ .set__description("The topic where the laser scan will be published"));
  this->get_parameter("scan_topic", scan_topic_);
  RCLCPP_INFO(this->get_logger(), "The parameter scan_topic is set to: %s", scan_topic_.c_str());
 
- nav2_util::declare_parameter_if_not_declared(this, "frame_id", rclcpp::ParameterValue("base_laser_link"), 
- rcl_interfaces::msg::ParameterDescriptor()
- .set__description("The frame of the scanner"));
+ nav2_util::declare_parameter_if_not_declared(this, 
+ "frame_id", rclcpp::ParameterValue("base_laser_link"), 
+ rcl_interfaces::msg::ParameterDescriptor()
+ .set__description("The frame of the scanner"));
  this->get_parameter("frame_id", frame_id_);
  RCLCPP_INFO(this->get_logger(), "The parameter frame_id is set to: %s", frame_id_.c_str());
 
- nav2_util::declare_parameter_if_not_declared(this, "scan_duration", rclcpp::ParameterValue(0.025), 
- rcl_interfaces::msg::ParameterDescriptor()
- .set__description("Time between laser scans"));
+ nav2_util::declare_parameter_if_not_declared(this, 
+ "scan_duration", rclcpp::ParameterValue(0.025), 
+ rcl_interfaces::msg::ParameterDescriptor()
+ .set__description("Time between laser scans"));
  this->get_parameter("scan_duration", scan_duration_);
  RCLCPP_INFO(this->get_logger(), "The parameter scan_duration is set to: %f", scan_duration_);
 
- nav2_util::declare_parameter_if_not_declared(this, "scan_cycle_time", rclcpp::ParameterValue(0.040), 
- rcl_interfaces::msg::ParameterDescriptor()
- .set__description("Cycle time of the scan"));
+ nav2_util::declare_parameter_if_not_declared(this, 
+ "scan_cycle_time", rclcpp::ParameterValue(0.040), 
+ rcl_interfaces::msg::ParameterDescriptor()
+ .set__description("Cycle time of the scan"));
  this->get_parameter("scan_cycle_time", scan_cycle_time_);
- RCLCPP_INFO(this->get_logger(), "The parameter scan_cycle_time is set to: %f", scan_cycle_time_);
-
- nav2_util::declare_parameter_if_not_declared(this, "debug", rclcpp::ParameterValue(false), 
- rcl_interfaces::msg::ParameterDescriptor()
- .set__description("Option to toggle scanner debugging information"));
+ RCLCPP_INFO(this->get_logger(), 
+ "The parameter scan_cycle_time is set to: %f", scan_cycle_time_);
+
+ nav2_util::declare_parameter_if_not_declared(this, 
+ "scan_delay", rclcpp::ParameterValue(0.075), 
+ rcl_interfaces::msg::ParameterDescriptor()
+ .set__description("Delay of the scan"));
+ this->get_parameter("scan_delay", scan_delay_);
+ RCLCPP_INFO(this->get_logger(), 
+ "The parameter scan_delay is set to: %f", scan_delay_);
+
+ nav2_util::declare_parameter_if_not_declared(this, 
+ "debug", rclcpp::ParameterValue(false), 
+ rcl_interfaces::msg::ParameterDescriptor()
+ .set__description("Option to toggle scanner debugging information"));
  this->get_parameter("debug", debug_);
- RCLCPP_INFO(this->get_logger(), "The parameter debug is set to: %s", debug_ ? "true" : "false");
+ RCLCPP_INFO(this->get_logger(), 
+ "The parameter debug is set to: %s", debug_ ? "true" : "false");
 
- nav2_util::declare_parameter_if_not_declared(this, "communication_timeout", rclcpp::ParameterValue(0.2), 
- rcl_interfaces::msg::ParameterDescriptor()
- .set__description("Timeout to shutdown the node"));
+ nav2_util::declare_parameter_if_not_declared(this, 
+ "communication_timeout", rclcpp::ParameterValue(0.2), 
+ rcl_interfaces::msg::ParameterDescriptor()
+ .set__description("Timeout to shutdown the node"));
  this->get_parameter("communication_timeout", communication_timeout_);
- RCLCPP_INFO(this->get_logger(), "The parameter communication_timeout is set to: %f", communication_timeout_);
+ RCLCPP_INFO(this->get_logger(), 
+ "The parameter communication_timeout is set to: %f", communication_timeout_);
 
  // Read 'fields' params. Set 1 by default to be backwards compatible
  // TODO: Change this when ROS will support YAML mixed types
  ScannerSickS300::ParamType param;
  param.range_field = 1;
- nav2_util::declare_parameter_if_not_declared(this, "fields.1.scale", rclcpp::ParameterValue(0.01), 
- rcl_interfaces::msg::ParameterDescriptor()
- .set__description("Scale of the field"));
+ nav2_util::declare_parameter_if_not_declared(this, 
+ "fields.1.scale", rclcpp::ParameterValue(0.01), 
+ rcl_interfaces::msg::ParameterDescriptor()
+ .set__description("Scale of the field"));
  this->get_parameter("fields.1.scale", param.dScale);
  RCLCPP_INFO(this->get_logger(), "The parameter field.1.scale is set to: %f", param.dScale);
 
- nav2_util::declare_parameter_if_not_declared(this, "fields.1.start_angle", rclcpp::ParameterValue(-135.0 / 180.0 * M_PI), 
- rcl_interfaces::msg::ParameterDescriptor()
- .set__description("Start angle of the field"));
+ nav2_util::declare_parameter_if_not_declared(this, 
+ "fields.1.start_angle", rclcpp::ParameterValue(-135.0 / 180.0 * M_PI), 
+ rcl_interfaces::msg::ParameterDescriptor()
+ .set__description("Start angle of the field"));
  this->get_parameter("fields.1.start_angle", param.dStartAngle);
- RCLCPP_INFO(this->get_logger(), "The parameter field.1.start_angle is set to: %f", param.dStartAngle);
+ RCLCPP_INFO(this->get_logger(), 
+ "The parameter field.1.start_angle is set to: %f", param.dStartAngle);
 
- nav2_util::declare_parameter_if_not_declared(this, "fields.1.stop_angle", rclcpp::ParameterValue(135.0 / 180.0 * M_PI), 
- rcl_interfaces::msg::ParameterDescriptor()
- .set__description("Stop angle of the field"));
+ nav2_util::declare_parameter_if_not_declared(this, 
+ "fields.1.stop_angle", rclcpp::ParameterValue(135.0 / 180.0 * M_PI), 
+ rcl_interfaces::msg::ParameterDescriptor()
+ .set__description("Stop angle of the field"));
  this->get_parameter("fields.1.stop_angle", param.dStopAngle);
- RCLCPP_INFO(this->get_logger(), "The parameter field.1.stop_angle is set to: %f", param.dStopAngle);
+ RCLCPP_INFO(this->get_logger(), 
+ "The parameter field.1.stop_angle is set to: %f", param.dStopAngle);
  scanner_.setRangeField(1, param);
 
  // Configure the publishers
- laser_scan_pub_ = this->create_publisher<sensor_msgs::msg::LaserScan>(scan_topic_, rclcpp::SensorDataQoS());
- in_standby_pub_ = this->create_publisher<std_msgs::msg::Bool>("scan_standby", rclcpp::QoS(1).transient_local());
- diag_pub_ = this->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", rclcpp::QoS(1));
+ laser_scan_pub_ = this->create_publisher<sensor_msgs::msg::LaserScan>(
+ scan_topic_, rclcpp::SensorDataQoS());
+ in_standby_pub_ = this->create_publisher<std_msgs::msg::Bool>(
+ "scan_standby", rclcpp::QoS(1).transient_local());
+ diag_pub_ = this->create_publisher<diagnostic_msgs::msg::DiagnosticArray>(
+ "/diagnostics", rclcpp::QoS(1));
 
  // Open the laser scanner
  bool bOpenScan = this->open();
  if (!bOpenScan){
- RCLCPP_ERROR(this->get_logger(), "...scanner not available on port %s. Please, try again.", port_.c_str());
+ RCLCPP_ERROR(this->get_logger(), 
+ "...scanner not available on port %s. Please, try again.", port_.c_str());
  return rclcpp_CallReturn::FAILURE;
  }else{
  // Wait for scan to get ready if successful
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
- RCLCPP_INFO(this->get_logger(), "...scanner opened successfully on port %s", port_.c_str());
+ RCLCPP_INFO(this->get_logger(), 
+ "...scanner opened successfully on port %s", port_.c_str());
 
  return rclcpp_CallReturn::SUCCESS;
  }
@@ -146,7 +178,8 @@ rclcpp_CallReturn SickS3002::on_activate(const rclcpp_lifecycle::State & state){
  LifecycleNode::on_activate(state);
  RCLCPP_INFO(this->get_logger(), "Activating the node...");
 
- timer_ = this->create_wall_timer(std::chrono::duration<double>(scan_cycle_time_), std::bind(&SickS3002::receiveScan, this));
+ timer_ = this->create_wall_timer(std::chrono::duration<double>(scan_cycle_time_), 
+ std::bind(&SickS3002::receiveScan, this));
 
  return rclcpp_CallReturn::SUCCESS;
 }
@@ -195,13 +228,15 @@ bool SickS3002::receiveScan(){
  std::vector<double> ranges, rangeAngles, intensities;
  unsigned int iSickTimeStamp, iSickNow;
 
- int result = scanner_.getScan(ranges, rangeAngles, intensities, iSickTimeStamp, iSickNow, debug_);
+ int result = scanner_.getScan(ranges, rangeAngles, intensities, 
+ iSickTimeStamp, iSickNow, debug_);
  static rclcpp::Time pointTimeCommunicationOK(this->now());
 
  if (result){
  if (scanner_.isInStandby()){
  publishWarn("scanner in standby");
- RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 30, "scanner on port %s in standby", port_.c_str());
+ RCLCPP_WARN_THROTTLE(this->get_logger(), 
+ *this->get_clock(), 30, "scanner on port %s in standby", port_.c_str());
  publishStandby(true);
  }else{
  publishStandby(false);
@@ -226,7 +261,8 @@ void SickS3002::publishStandby(bool in_standby){
  in_standby_pub_->publish(in_standby_);
 }
 
-void SickS3002::publishLaserScan(std::vector<double> vdDistM, std::vector<double> vdAngRAD, std::vector<double> vdIntensAU, unsigned int iSickTimeStamp, unsigned int iSickNow){
+void SickS3002::publishLaserScan(std::vector<double> vdDistM, std::vector<double> vdAngRAD, 
+ std::vector<double> vdIntensAU, unsigned int iSickTimeStamp, unsigned int iSickNow){
  // Fill message
  int start_scan = 0;
  int num_readings = vdDistM.size(); // initialize with max scan size
@@ -250,7 +286,8 @@ void SickS3002::publishLaserScan(std::vector<double> vdDistM, std::vector<double
  double timeDiff = static_cast<int>(iSickTimeStamp - synced_sick_stamp_) * scan_cycle_time_;
  laserScan.header.stamp = synced_ros_time_ + rclcpp::Duration::from_seconds(timeDiff);
 
- RCLCPP_DEBUG(this->get_logger(), "Time::now() - calculated sick time stamp = %f",(this->now() - laserScan.header.stamp).seconds());
+ RCLCPP_DEBUG(this->get_logger(), "Time::now() - calculated sick time stamp = %f", 
+ (this->now() - laserScan.header.stamp).seconds());
  }else{
  laserScan.header.stamp = this->now();
  }
@@ -272,10 +309,13 @@ void SickS3002::publishLaserScan(std::vector<double> vdDistM, std::vector<double
  // Check for inverted laser
  if (inverted_){
  // to be really accurate, we now invert time_increment
- // laserScan.header.stamp = rclcpp::Time(laserScan.header.stamp) + rclcpp::Duration::from_seconds(scanDuration_); //adding of the sum over all negative increments would be mathematically correct, but looks worse.
+ // laserScan.header.stamp = rclcpp::Time(laserScan.header.stamp) + 
+ // rclcpp::Duration::from_seconds(scanDuration_); //adding of the sum over all negative increments would be mathematically correct, but looks worse.
  laserScan.time_increment = - laserScan.time_increment;
  }else{
- laserScan.header.stamp = rclcpp::Time(laserScan.header.stamp) - rclcpp::Duration::from_seconds(scan_duration_); //to be consistent with the omission of the addition above
+ laserScan.header.stamp = rclcpp::Time(laserScan.header.stamp) - 
+ rclcpp::Duration::from_seconds(scan_duration_) - 
+ rclcpp::Duration::from_seconds(scan_delay_); //to be consistent with the omission of the addition above
  }
 
  for (int i = 0; i < (stop_scan - start_scan); i++){