Fixed segmentation fault in depth_image_octomap_updater

[CihatAltiparmak]

Description

The relavant issue: #2964

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference
• Document API changes relevant to the user in the MIGRATION.md notes
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[rhaschke]

Please always explain your changes!
This looks like a great data type mess: the data array of type uint8 should store floats (TYPE_32FC1), but is passed as a double pointer to getModelDepth(), which might be the root cause of the segfault.
Now, you allocate an additional float array (which takes extra time and memory), still pass it as a double pointer (messing up the data) and finally copy the float data as unsigned short into the uint8 slots using some weird transformation. As the uint8 array is to be interpreted as a float array, this is definitely still wrong.

Have a look at the MoveIt1 implementation of that snippet, which seems to use the correct types.

[CihatAltiparmak]

Now, you allocate an additional float array (which takes extra time and memory), still pass it as a double pointer (messing up the data) and finally copy the float data as unsigned short into the uint8 slots using some weird transformation.

It actually comes from

moveit2/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp

Lines 468 to 492 in aece55c

	if (!filtered_cloud_topic_.empty())
	{
	sensor_msgs::msg::Image filtered_msg;
	filtered_msg.header = depth_msg->header;
	filtered_msg.height = h;
	filtered_msg.width = w;
	filtered_msg.is_bigendian = HOST_IS_BIG_ENDIAN;
	filtered_msg.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
	filtered_msg.step = w * sizeof(unsigned short);
	filtered_msg.data.resize(img_size * sizeof(unsigned short));
	// reuse float buffer across callbacks
	static std::vector<float> filtered_data;
	if (filtered_data.size() < img_size)
	filtered_data.resize(img_size);
	mesh_filter_->getFilteredDepth(reinterpret_cast<double*>(&filtered_data[0]));
	unsigned short* msg_data = reinterpret_cast<unsigned short*>(&filtered_msg.data[0]);
	for (std::size_t i = 0; i < img_size; ++i)
	{
	// rescale depth to millimeter to work with `unsigned short`
	msg_data[i] = static_cast<unsigned short>(filtered_data[i] * 1000 + 0.5);
	}
	pub_filtered_depth_image_.publish(filtered_msg, *info_msg);
	}

Have a look at the MoveIt1 implementation of that snippet, which seems to use the correct types.

Hmm, thanks, i will give a try converting getModelDepth(double) to getModelDepth(float). and the i will let you know about it.

[rhaschke]

The referenced code looks wrong as well. Again, MoveIt1 code seems to be correct. Note the key difference that the referenced code snippet generates a 16bit unsigned int encoding (TYPE_16UC1).

[CihatAltiparmak]

I've updated based on Moveit1's implementation and it works.
Should we remove this parts too (Moveit1 has same implementation), what do you think @rhaschke ? Btw maybe we must add this modifications to migration guide.

moveit2/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp

Lines 486 to 490 in aece55c

	for (std::size_t i = 0; i < img_size; ++i)
	{
	// rescale depth to millimeter to work with `unsigned short`
	msg_data[i] = static_cast<unsigned short>(filtered_data[i] * 1000 + 0.5);
	}

[rhaschke]

This looks better now. Now idea, why #2343 blindly converted all float args to double.

[rhaschke]

Should we remove this parts too (Moveit1 has same implementation)?

No. This code explicitly returns an unsigned short array, i.e. converting float meters into int millimeters.

[sjahr]

This looks better now. Now idea, why #2343 blindly converted all float args to double.

Thanks for referencing moveit1. The reason for the conversation is: https://github.com/cpp-best-practices/cppbestpractices/blob/master/08-Considering_Performance.md#prefer-double-to-float-but-test-first but apparently we ignore the test_first part 🤦‍♂️

[CihatAltiparmak]

I have surfed through the PR you mentioned and i also discovered that the evil is also here ( #1578 ) It can be necessary to revert some float-to-double conversions