search

yujinrobot / yujin_ocs

Yujin Robot's open-source control libraries
http://wiki.ros.org/yujin_ocs
235 stars 178 forks source link

OdometryHelper when odom not yet available #110

Closed AlexReimann closed 7 years ago

AlexReimann commented 7 years ago

The OdometryHelper currently gives a non normalized quaternion as orientation when the odometry is requested when no odometry has been published yet. This is because the odometry_ variable is initialized with the default which is [0 0 0 0]. This leads to some problems and maybe nans when it is used in the code.

So should properly initialize it or throw an error etc. There is actually not a really a reasonable default initialization for that variable (but a normalized quaternion is better than getting some nans).

AlexReimann commented 7 years ago

fyi @bit-pirate

AlexReimann commented 7 years ago

Updated the description.

The problem mainly is that our trajectory controller uses the current odom and the past issued commands to estimate where the robot will be in the future. This estimate is then passed to the local planner which leads to the local planner running into problems because the pose is faulty (a non-normalized quaternion as orientation)

AlexReimann commented 7 years ago

I'll catch this for now by checking for a non-normalized quaternion. Maybe this is the actual intent behind the default quaternion constructor leading to an invalid quaternion (or rather not normalized). Maybe close this..?

stonier commented 7 years ago

Sending out a non-normalised quaternion and then requiring a user to interpret that as uninitialised would be quite bad design. There are much easier ways to help a user understand its uninitialised than that.

Initialising it as well with [0 0 0 1] would also be quite bad design. There are valid use cases where the user would need to know if it's initialised or not, and that is not enough information for those cases.

I'd say the cleanest solution is to have the odometry in a std shared pointer which only gets initialised after it's got data (user can check the shared pointer himself) and then move the position/velocity processing methods to library functions. Separate data collection and data processing concerns.

Otherwise use exceptions in each method for when it's uninitialised. Or a combination of the shared pointer/exception idea.

Keep in mind that exceptions are usually an expensive option. For control systems I typically only take advantage of their usefulness in constructor or init functions.

stonier commented 7 years ago

I don't think there is anything special about these classes - I just pulled them from your clrrt just after you finished your master's period so I could re-use them in various places.

Feel free to take ownership of what is in this package.

AlexReimann commented 7 years ago

Ohh, so it's my shitty code. I see..

AlexReimann commented 7 years ago

Fixed with pr #119