use helper function strToDouble for locale independent parsing of colors

[simonschmeisser]

This commit fixes #46 by using strToDouble instead of the locale dependent std::stof

[simonschmeisser]

Now this PR has actually been compiled and tested and successfully loads colored robots! 👍

[simonschmeisser]

@scpeters can we repeat the game with this fix?

[clalancette]

Arg. When I did #42, I explicitly looked for all instances of std::stod in the urdf codebase and replaced them, but I forgot to look for std::stof. So thanks for catching this @simonschmeisser .

My hesitation with this change is that doing a conversion between string and double, then storing it into a float, has the possibility of running into undefined territory (ala https://msdn.microsoft.com/en-us/library/d3d6fhea.aspx). After discussing it with @sloretz, I think the better thing to do is to add a strToFloat method to https://github.com/ros/urdfdom_headers/blob/master/urdf_model/include/urdf_model/utils.h which calls strToDouble, then does a range check to see if the conversion would be outside the bounds of a float. If it is, it throws a std::runtime_error, if it doesn't, it returns the float. Then we change the code in https://github.com/ros/urdfdom_headers/blob/master/urdf_model/include/urdf_model/color.h to use strToFloat. It's a bit more work, but it ensures we don't get into undefined territory later.

@simonschmeisser , do you mind doing that change?

[simonschmeisser]

@clalancette sure, I'll look at this on Monday if nobody beats me to it ...

One question (without reading that link ...) why don't we do the conversion to float directly but add the double conversion and check in between?

[clalancette]

@clalancette sure, I'll look at this on Monday if nobody beats me to it ...

One question (without reading that link ...) why don't we do the conversion to float directly but add the double conversion and check in between?

We were mostly trying to avoid having the duplicated code between strToDouble and (the new) strToFloat, since they are going to look exactly the same except for the return type. We could do something with templates and template specialization, but at that point we realized it was just as easy to use strToDouble and add a range check.

[simonschmeisser]

see moveit/moveit#1099 for the template variant as proposed by @rhascke. I was just going for the copy paste initially but actually don't care really

howerever I still think that range check thing is a strange way

[clalancette]

see ros-planning/moveit#1099 for the template variant as proposed by @rhascke. I was just going for the copy paste initially but actually don't care really

howerever I still think that range check thing is a strange way

If you want to go for the templated version, that is fine with me. However, I will point out that the version in moveit/moveit#1099 isn't actually sufficient; that will take any templated type, including nonsensical things like:

toRealImpl<std::string>("0.1");

To make the templated version always do what it says, you'll have to do template specialization with some kind of STATIC_ASSERT or std::enable_if to ensure you only specialize on the types you want (double and float). This is why I was trying to avoid the template :).

[simonschmeisser]

that's the reason why that template is not exposed and just available in the source file :)

(besides, that nonsense example would still return valid results)

[clalancette]

that's the reason why that template is not exposed and just available in the source file :)

Good point, I had missed that.

[simonschmeisser]

I guess this is not what you originally meant but I thought I propose it anyway since stricter parsing is always a good idea and the range [0, 1] is mentioned in the urdf specification http://wiki.ros.org/urdf/XML/link

[clalancette]

I guess this is not what you originally meant but I thought I propose it anyway since stricter parsing is always a good idea and the range [0, 1] is mentioned in the urdf specification http://wiki.ros.org/urdf/XML/link

Yeah, this was the other way @sloretz and I had discussed. While I agree with you that this is more correct, our concern here was that we'll start rejecting URDF that previously worked in stable ROS distros. In thinking about it more, however, this change targetting master will really only affect Melodic, and that is "new enough" that I think this change is warranted there. I'll be much more concerned once someone proposes backporting this to Kinetic, but we can cross that bridge when we get to it.

@sloretz @scpeters What I'm saying here is that I'm fine with this change as-is, targeted at master. What are your thoughts?

[sloretz]

In thinking about it more, however, this change targetting master will really only affect Melodic, and that is "new enough" that I think this change is warranted there.

I agree the range check is closer to the spec. @j-rivero do you think fixing the locale issue by adding a range check would be too much of a change for an SRU (#45)?

[j-rivero]

@j-rivero do you think fixing the locale issue by adding a range check would be too much of a change for an SRU (#45)?

Not sure about what kind of check or modifications it would be, may I see the patch? Going beyond the code change, I think that a test covering this specific issue would help and the final word is not really mine.

[sloretz]

Not sure about what kind of check or modifications it would be, may I see the patch?

It's the current state of this PR, changing

          rgba.push_back(std::stof(pieces[i]));

to

          double piece = strToDouble(pieces[i].c_str());
          if ((piece < 0) || (piece > 1))
            throw ParseError("Component [" + pieces[i] + "] is outside the valid range for colors [0, 1]");
          rgba.push_back(piece);

It simultaneously fixes two issues: replaces std::stof to avoid being dependent on locale, and disallows creating of a color with component value greater than 1.0. If the justification for the SRU is given as fixing the first issue, would submitting the current PR as a patch hurt the chances of getting an SRU through?

[simonschmeisser]

@j-rivero would the test in ros/urdfdom#115 be suitable?

[simonschmeisser]

@scpeters @sloretz @clalancette can we move ahead with this so that a new patch release can be submitted to Ubuntu/Debian by @j-rivero ?

[scpeters]

I'm going to squash 629a872 and 4d074c8 together

[scpeters]

@j-rivero please use f97cbce for the patch release

[simonschmeisser]

Thanks a lot! @j-rivero please tell me if you need any further help with updating this in ubuntu

[sloretz]

Looks like this line warns on windows

c:\j\workspace\ci_packaging_windows\ws\install\include\urdf_model\color.h(79): warning C4244: 'argument': conversion from 'double' to 'const float', possible loss of data [C:\J\workspace\ci_packaging_windows\ws\build\robot_state_publisher\robot_state_publisher_solver.vcxproj

https://ci.ros2.org/job/ci_packaging_windows/82

[clalancette]

Ug. Well, I guess we can static_cast<float> here; we know it will be safe.

[sloretz]

#48

[j-rivero]

@j-rivero would the test in ros/urdfdom#115 be suitable?

It will need a bit of clean up but I think it is fine, thanks. We will need two different things for Ubuntu and Debian:

• Ubuntu: I will send the SRU bug with the patch (hopefully next week). I assume that you guys want the SRU bug against Bionic, right?
• Debian: @scpeters can we release the changes under a new patch version? Changes will hit Debian Sid immediately and Ubuntu new releases will sync from it.

[simonschmeisser]

Bionic is correct, yes. While I personally don't care we should probably also consider Bionic+0.5/18.10 as there will be bug reports and confusion from people using that. But maybe one after the other

Thanks!

[scpeters]

@j-rivero sure I'll make a new patch release, maybe after fixing the windows compiler warnings

[scpeters]

@j-rivero I just released 1.0.2

[j-rivero]

@j-rivero would the test in ros/urdfdom#115 be suitable?

It will need a bit of clean up but I think it is fine, thanks

I'm probably doing something wrong for creating an standalone test that demostrate the failure:

#include <iostream>
#include "urdf_parser/urdf_parser.h"


int main()
{
  setlocale(LC_ALL, "");

  std::string joint_str =
    "<robot name=\"test\">"
    "  <joint name=\"j1\" type=\"fixed\">"
    "    <parent link=\"l1\"/>"
    "    <child link=\"l2\"/>"
    "  </joint>"
    "  <joint name=\"j2\" type=\"fixed\">"
    "    <parent link=\"l1\"/>"
    "    <child link=\"l2\"/>"
    "  </joint>"
    "  <link name=\"l1\">"
    "    <visual>"
    "      <geometry>"
    "        <sphere radius=\"1.349\"/>"
    "      </geometry>"
    "      <material name=\"\">"
    "        <color rgba=\"1.0 0.65 0.0 0.01\" />"
    "      </material>"
    "    </visual>"
    "    <inertial>"
    "      <mass value=\"8.4396\"/>"
    "      <inertia ixx=\"0.087\" ixy=\"0.14\" ixz=\"0.912\" iyy=\"0.763\" iyz=\"0.0012\" izz=\"0.908\"/>"
    "    </inertial>"
    "  </link>"
    "  <link name=\"l2\">"
    "    <visual>"
    "      <geometry>"
    "        <cylinder radius=\"3.349\" length=\"7.5490\"/>"
    "      </geometry>"
    "      <material name=\"red ish\">"
    "        <color rgba=\"1 0.0001 0.0 1\" />"
    "      </material>"
    "    </visual>"
    "  </link>"
    "</robot>";

  urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str);

  std::shared_ptr<urdf::Sphere> s = std::dynamic_pointer_cast<urdf::Sphere>(urdf->links_["l1"]->visual->geometry);
  return 0;
}

Compiled with g++ foo.cpp $(pkg-config --cflags --libs urdfdom) -o foo

Using locales:

LANG=nl_NL
LANGUAGE=
LC_CTYPE="nl_NL"
LC_NUMERIC="nl_NL"
LC_TIME="nl_NL"
LC_COLLATE="nl_NL"
LC_MONETARY="nl_NL"
LC_MESSAGES="nl_NL"
LC_PAPER="nl_NL"
LC_NAME="nl_NL"
LC_ADDRESS="nl_NL"
LC_TELEPHONE="nl_NL"
LC_MEASUREMENT="nl_NL"
LC_IDENTIFICATION="nl_NL"
LC_ALL=nl_NL

I don't see any obvious failure.

[simonschmeisser]

hmm, where would you expect a failure here? all values < 1.0 are truncated to 0 but that does not trigger any errors. So you would need to add some check to test for correctness (!= 0) of the parsed values.

[j-rivero]

hmm, where would you expect a failure here? all values < 1.0 are truncated to 0 but that does not trigger any errors. So you would need to add some check to test for correctness (!= 0) of the parsed values.

Thanks Simon, that makes sense I did not follow the origin of the issue. For some reason if I use this PR to patch the stable version on Bionic (1.0.0) the error seems to be still present. This is the patch:

diff --git a/urdf_model/include/urdf_model/color.h b/urdf_model/include/urdf_model/color.h
index 505d9a6..fec8ae0 100644
--- a/urdf_model/include/urdf_model/color.h
+++ b/urdf_model/include/urdf_model/color.h
@@ -73,13 +73,13 @@ public:
       {
         try
         {
-          rgba.push_back(std::stof(pieces[i]));
+          double piece = strToDouble(pieces[i].c_str());
+          if ((piece < 0) || (piece > 1))
+            throw ParseError("Component [" + pieces[i] + "] is outside the valid range for colors [0, 1]");
+          rgba.push_back(piece);
         }
-        catch (std::invalid_argument &/*e*/) {
-          return false;
-        }
-        catch (std::out_of_range &/*e*/) {
-          return false;
+        catch (std::runtime_error &/*e*/) {
+          throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a color value)");
         }
       }
     }
diff --git a/urdf_model/include/urdf_model/utils.h b/urdf_model/include/urdf_model/utils.h
index e295496..b4843c5 100644
--- a/urdf_model/include/urdf_model/utils.h
+++ b/urdf_model/include/urdf_model/utils.h
@@ -37,6 +37,10 @@
 #ifndef URDF_INTERFACE_UTILS_H
 #define URDF_INTERFACE_UTILS_H
 
+
+#include <locale>
+#include <sstream>
+#include <stdexcept>
 #include <string>
 #include <vector>
 
@@ -62,6 +66,29 @@ void split_string(std::vector<std::string> &result,
   }
 }
 
+
+// This is a locale-safe version of string-to-double, which is suprisingly
+// difficult to do correctly.  This function ensures that the C locale is used
+// for parsing, as that matches up with what the XSD for double specifies.
+// On success, the double is returned; on failure, a std::runtime_error is
+// thrown.
+static inline double strToDouble(const char *in)
+{
+  std::stringstream ss;
+  ss.imbue(std::locale::classic());
+
+  ss << in;
+
+  double out;
+  ss >> out;
+
+  if (ss.fail() || !ss.eof()) {
+    throw std::runtime_error("Failed converting string to double");
+  }
+
+  return out;
+}
+
 }
 
 #endif

and my testing code:

#include <iostream>
#include "urdf_parser/urdf_parser.h"

int main()
{
  setlocale(LC_ALL, "");

  std::string joint_str =
    "<robot name=\"test\">"
    "  <joint name=\"j1\" type=\"fixed\">"
    "    <parent link=\"l1\"/>"
    "    <child link=\"l2\"/>"
    "  </joint>"
    "  <joint name=\"j2\" type=\"fixed\">"
    "    <parent link=\"l1\"/>"
    "    <child link=\"l2\"/>"
    "  </joint>"
    "  <link name=\"l1\">"
    "    <visual>"
    "      <geometry>"
    "        <sphere radius=\"1.349\"/>"
    "      </geometry>"
    "      <material name=\"\">"
    "        <color rgba=\"1.0 0.65 0.0 0.01\" />"
    "      </material>"
    "    </visual>"
    "    <inertial>"
    "      <mass value=\"8.4396\"/>"
    "      <inertia ixx=\"0.087\" ixy=\"0.14\" ixz=\"0.912\" iyy=\"0.763\" iyz=\"0.0012\" izz=\"0.908\"/>"
    "    </inertial>"
    "  </link>"
    "  <link name=\"l2\">"
    "    <visual>"
    "      <geometry>"
    "        <cylinder radius=\"3.349\" length=\"7.5490\"/>"
    "      </geometry>"
    "      <material name=\"red ish\">"
    "        <color rgba=\"1 0.0001 0.0 1\" />"
    "      </material>"
    "    </visual>"
    "  </link>"
    "</robot>";

  urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str);
  if ((urdf->links_["l1"]->visual->material->color.a == 0.01f) &&
      (urdf->links_["l1"]->visual->material->color.g == 0.65f))
    std::cout << "OK" << std::endl;
  else
    std::cout << "ERROR" << std::endl;

  return 0;
}

I'm probably doing something wrong but can not find what.

[simonschmeisser]

Looks reasonable. Did you clean everything? Rebuild everything? I was recently missing the package.xml so it was using the packaged one instead of the one in my catkin workspace? But I'm really just guessing.

Is it really that much harder to update to 1.0.2 instead of doing a patch on 1.0.0? (Which would probably still need a testcase right?)

[simonschmeisser]

@j-rivero whats the status on the update?

[j-rivero]

@j-rivero whats the status on the update?

@simonschmeisser the easiest way to have an SRU is to minimize the number of changes that is why I was working on only porting the related code. Getting in a patch version which ships more changes is pretty complicated. Back to the issue, could you please provide a Dockerfile with the steps that demonstrate the error and fix? I was unable to get it working on my system after trying rebuilds, locale changes, etc. If you do it, I will take care of the rest of the SRU.