Fix yaml parser error when meets .nan (refactor on #754)

rcl_yaml_param_parser/src/parse.c

@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <ctype.h>
 #include <errno.h>
 
 #include "rcutils/allocator.h"
@@ -116,7 +117,30 @@ void * get_value(
   {
     errno = 0;
     endptr = NULL;
-    dval = strtod(value, &endptr);
+    const char * iter_ptr = NULL;
+    if ((0 == strcmp(value, ".nan")) ||
+      (0 == strcmp(value, ".NaN")) ||
+      (0 == strcmp(value, ".NAN")) ||
+      (0 == strcmp(value, ".inf")) ||
+      (0 == strcmp(value, ".Inf")) ||
+      (0 == strcmp(value, ".INF")) ||
+      (0 == strcmp(value, "+.inf")) ||
+      (0 == strcmp(value, "+.Inf")) ||
+      (0 == strcmp(value, "+.INF")) ||
+      (0 == strcmp(value, "-.inf")) ||
+      (0 == strcmp(value, "-.Inf")) ||
+      (0 == strcmp(value, "-.INF")))
+    {
+      for (iter_ptr = value; !isalpha(*iter_ptr); ) {
+        iter_ptr += 1;
+      }
+      dval = strtod(iter_ptr, &endptr);
+      if (*value == '-') {
+        dval = -dval;
+      }
+    } else {
+      dval = strtod(value, &endptr);
+    }
     if ((0 == errno) && (NULL != endptr)) {
       if ((NULL != endptr) && (endptr != value)) {
         if (('\0' != *value) && ('\0' == *endptr)) {

rcl_yaml_param_parser/src/parser.c

@@ -21,6 +21,7 @@
 #include <stdlib.h>
 #include <string.h>
 #include <yaml.h>
+#include <ctype.h>
 
 #include "rcl_yaml_param_parser/parser.h"
 #include "rcl_yaml_param_parser/types.h"

rcl_yaml_param_parser/test/special_float.yaml

@@ -0,0 +1,4 @@
+test_node:
+    ros__parameters:
+        isstring: [string, .nananan, .nAN, .Nan, .infinf, .INf, .infinity]
+        nan_inf: [1.1, 2.2, .nan, .NAN, .inf, +.Inf, -.INF]

rcl_yaml_param_parser/test/test_parse_yaml.cpp

@@ -12,9 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <stdio.h>
 #include <gtest/gtest.h>
 
+#include <cmath>
+
 #include "osrf_testing_tools_cpp/scope_exit.hpp"
 
 #include "rcl_yaml_param_parser/parser.h"
@@ -511,6 +512,53 @@ TEST(test_file_parser, maximum_number_parameters) {
   EXPECT_FALSE(res);
 }
 
+// Test special float point(https://github.com/ros2/rcl/issues/555).
+TEST(test_file_parser, special_float_point) {
+  rcutils_reset_error();
+  EXPECT_TRUE(rcutils_get_cwd(cur_dir, 1024)) << rcutils_get_error_string().str;
+  rcutils_allocator_t allocator = rcutils_get_default_allocator();
+  char * test_path = rcutils_join_path(cur_dir, "test", allocator);
+  ASSERT_TRUE(NULL != test_path) << rcutils_get_error_string().str;
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+  {
+    allocator.deallocate(test_path, allocator.state);
+  });
+  char * path = rcutils_join_path(test_path, "special_float.yaml", allocator);
+  ASSERT_TRUE(NULL != path) << rcutils_get_error_string().str;
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+  {
+    allocator.deallocate(path, allocator.state);
+  });
+  ASSERT_TRUE(rcutils_exists(path)) << "No test YAML file found at " << path;
+  rcl_params_t * params_hdl = rcl_yaml_node_struct_init(allocator);
+  ASSERT_TRUE(NULL != params_hdl) << rcutils_get_error_string().str;
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+  {
+    rcl_yaml_node_struct_fini(params_hdl);
+  });
+
+  bool res = rcl_parse_yaml_file(path, params_hdl);
+  EXPECT_TRUE(res) << rcutils_get_error_string().str;
+  rcl_variant_t * param_value = rcl_yaml_node_struct_get("test_node", "isstring", params_hdl);
+  ASSERT_TRUE(NULL != param_value) << rcutils_get_error_string().str;
+  ASSERT_TRUE(NULL != param_value->string_array_value);
+  EXPECT_STREQ(".nananan", param_value->string_array_value->data[1]);
+  EXPECT_STREQ(".nAN", param_value->string_array_value->data[2]);
+  EXPECT_STREQ(".infinf", param_value->string_array_value->data[4]);
+  EXPECT_STREQ(".INf", param_value->string_array_value->data[5]);
+  param_value = rcl_yaml_node_struct_get(
+    "test_node", "nan_inf", params_hdl);
+  ASSERT_TRUE(NULL != param_value) << rcutils_get_error_string().str;
+  ASSERT_TRUE(NULL != param_value->double_array_value);
+  ASSERT_EQ(7U, param_value->double_array_value->size);
+  EXPECT_FALSE(std::isnan(param_value->double_array_value->values[1]));
+  EXPECT_TRUE(std::isnan(param_value->double_array_value->values[2]));
+  EXPECT_TRUE(std::isnan(param_value->double_array_value->values[3]));
+  EXPECT_TRUE(std::isinf(param_value->double_array_value->values[4]));
+  EXPECT_TRUE(std::isinf(param_value->double_array_value->values[5]));
+  EXPECT_TRUE(std::isinf(param_value->double_array_value->values[6]));
+}
+
 int32_t main(int32_t argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);