-
Notifications
You must be signed in to change notification settings - Fork 412
/
unexpected_configuration_exception.h
125 lines (110 loc) · 5.28 KB
/
unexpected_configuration_exception.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2019. Toyota Research Institute
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of CNRS-LAAS and AIST nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/** @author Sean Curtis (sean@tri.global) */
#ifndef FCL_UNEXPECTED_CONFIGURATION_EXCEPTION_H
#define FCL_UNEXPECTED_CONFIGURATION_EXCEPTION_H
#include <exception>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <stdexcept>
#include <string>
#include "fcl/export.h"
namespace fcl {
namespace detail {
/** A custom exception type that can be thrown in low-level code indicating
that the program has reached an unexpected configuration and that the caller
that ultimately has access to the collision objects and their transforms
should report the configuration that led to the exception.
This is strictly an _internal_ exception; it should *always* be caught and
transformed into an exception that propagates to the operating system.
Recommended usage is to throw by invoking the macro
FCL_THROW_UNEXPECTED_CONFIGURATION_EXCEPTION defined below. Code that exercises
functionality that throws this type of exception should catch it and transform
it to a `std::logic_error` by invoking ThrowDetailedConfiguration(). */
class FCL_EXPORT UnexpectedConfigurationException final
: public std::exception {
public:
UnexpectedConfigurationException(const std::string& message)
: std::exception(), message_(message) {}
const char* what() const noexcept override { return message_.c_str(); }
private:
std::string message_;
};
/** Helper function for dispatching an `UnexpectedConfigurationException`.
* Because this exception is designed to be caught and repackaged, we lose the
* automatic association with file and line number. This wraps them into the
* message of the exception so it can be preserved in the re-wrapping.
*
* @param message The error message itself.
* @param func The name of the invoking function.
* @param file The name of the file associated with the exception.
* @param line The line number where the exception is thrown. */
FCL_EXPORT void ThrowUnexpectedConfigurationException(
const std::string& message, const char* func, const char* file, int line);
/** Helper class for propagating a low-level exception upwards but with
configuration-specific details appended. The parameters
@param s1 The first shape in the query.
@param X_FS1 The pose of the first shape in frame F.
@param s2 The second shape in the query.
@param X_FS2 The pose of the second shape in frame F.
@param solver The solver.
@param e The original exception.
@tparam Shape1 The type of shape for shape 1.
@tparam Shape2 The type of shape for shape 2.
@tparam Solver The solver type (with scalar type erase).
@tparam Pose The pose type (a Transform<S> with scalar type erased).
*/
template <typename Shape1, typename Shape2, typename Solver, typename Pose>
void ThrowDetailedConfiguration(const Shape1& s1, const Pose& X_FS1,
const Shape2& s2, const Pose& X_FS2,
const Solver& solver, const std::exception& e) {
std::stringstream ss;
ss << "Error with configuration"
<< "\n Original error message: " << e.what()
<< "\n Shape 1: " << s1
<< "\n X_FS1\n" << X_FS1.matrix()
<< "\n Shape 2: " << s2
<< "\n X_FS2\n" << X_FS2.matrix()
<< "\n Solver: " << solver;
throw std::logic_error(ss.str());
}
} // namespace detail
} // namespace fcl
#define FCL_THROW_UNEXPECTED_CONFIGURATION_EXCEPTION(message, func) \
::fcl::detail::ThrowUnexpectedConfigurationException(message, func, \
__FILE__, __LINE__)
// TODO(SeanCurtis-TRI): Add some helper functions to transform UCException into
// std::logic_error.
#endif // FCL_UNEXPECTED_CONFIGURATION_EXCEPTION_H