Update IR intensity sensor frame ID handling

[tanjammvo]

Description

The topic /ir_intensity publishes a vector containing a single intensity message instead of 7, even though there are 7 IR sensors.

The node that publishes the intensity value of each IR sensor individually publishes the message
without a frame_id . Therefore, the readings from all IR sensors share the "same" frame_id .

create3_sim/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/ir_intensity.cpp

Lines 56 to 77 in 6a8bc44

	auto ir_intensity_msg = irobot_create_msgs::msg::IrIntensity();
	const double detection =
	std::min(irobot_create_toolbox::FindMinimumRange(ir_msg->ranges), ir_msg->range_max);
	// IR sensor produces an exponential signal that is correlated to the distance,
	// that follows this formula: ir_reading = A exp(-x*B)
	// where:
	// A is a coefficient that depends on the color surface and
	// it can be as high as 3500
	// B is the decay of the signal related to the distance.
	// From the experiments B ~ 26.831568
	const double scaled_detection = 3500 * std::exp(detection * (-2 * M_E / ir_msg->range_max));
	ir_intensity_msg.value = static_cast<irobot_create_msgs::msg::IrIntensity::_value_type>(
	scaled_detection);
	// Publish to appropriate topic
	for (const std::string & sensor : ir_intensity_sensors_) {
	if (ir_msg->header.frame_id.find(sensor) != std::string::npos) {
	ir_intensity_pub_[sensor]->publish(ir_intensity_msg);
	}
	}

Subsequently, the node that aggregates these individual intensity values into a vector stores them in a map using the frame_id as the key. Consequently, every time this node receives a new reading from any of the IR sensors, it overwrites the same position in the map. The vector publisher node then periodically iterates through this map, builds a vector from its contents (resulting in only one intensity message), and publishes it to /ir_intensity .

create3_sim/irobot_create_common/irobot_create_nodes/src/ir_intensity_vector_publisher.cpp

Lines 54 to 65 in 6a8bc44

	// Create subscriptions
	for (std::string topic : subscription_topics_) {
	subs_vector_.push_back(
	(create_subscription<irobot_create_msgs::msg::IrIntensity>(
	topic, rclcpp::SensorDataQoS(),
	[this](const irobot_create_msgs::msg::IrIntensity::SharedPtr msg) {
	std::lock_guard<std::mutex> lock{this->mutex_};
	this->ir_intensities_[msg->header.frame_id] = *msg;
	})));
	RCLCPP_INFO_STREAM(get_logger(), "Subscription to topic: " << topic);
	}
	}

Fixes #209

• The node publishing intensity values uses now frame_id according the TF tree.

• Correct also the world name inside maze.sdf. maze instead of default. Otherwise /ir_intensity will not work with the world maze.
  

  create3_sim/irobot_create_ignition/irobot_create_ignition_bringup/worlds/maze.sdf

  Line 2 in 6a8bc44

  <world name='default'>

Type of change

• Bug fix (non-breaking change which fixes an issue)
• New feature (non-breaking change which adds functionality)

How Has This Been Tested?

Tests were conducted by launching the simulation as described in the documentation, and the verification of IR intensity values was done using PlotJuggler to ensure the correct structure of the intensity vector and the correctness of the range values.

Checklist

• I have performed a self-review of my own code
• I have commented my code, particularly in hard-to-understand areas
• (no needed) I have made corresponding changes to the documentation

[tanjammvo]

I think also that the fix can be applied to other branches/ROS distributions.