waitUntilFormed/Broken can raise exception while the state is changed (galactic/humble)

[daisukes]

bond_core/bondcpp/src/bond.cpp

Lines 395 to 418 in 52e181c

	bool Bond::waitUntilFormed(rclcpp::Duration timeout)
	{
	// std::unique_lock<std::mutex> lock(mutex_);
	rclcpp::Clock steady_clock(RCL_STEADY_TIME);
	rclcpp::Time deadline(steady_clock.now() + timeout);
	rclcpp::Rate r(100);
	while (sm_.getState().getId() == SM::WaitingForSister.getId()) {
	if (!rclcpp::ok()) {
	break;
	}
	rclcpp::Duration wait_time = rclcpp::Duration(100ms);
	if (timeout >= rclcpp::Duration(0.0s)) {
	rclcpp::Clock steady_clock(RCL_STEADY_TIME);
	wait_time = std::min(wait_time, deadline - steady_clock.now());
	}
	if (wait_time <= rclcpp::Duration(0.0s)) {
	break; // The deadline has expired
	}
	r.sleep();
	}
	return sm_.getState().getId() != SM::WaitingForSister.getId();
	}

sm_.getState() is not protected by the mutex and can be a null state.

This issue is addressed in the ros2 branch, but navigation2@humble does not seem to work with the latest bondcpp release.

So, I wanted to include the fix for humble release.