Skip to content

Commit

Permalink
Deprecate and remove all uses of DRAKE_ABORT_MSG (RobotLocomotion#10781)
Browse files Browse the repository at this point in the history
We want to remove DRAKE_ABORT_MSG, because in pydrake we should never
take down the interpreter.

Many uses are replaced by throwing an exception, or else perhaps by
using the DRAKE_UNREACHABLE macro.
  • Loading branch information
jwnimmer-tri authored Feb 28, 2019
1 parent e59be46 commit bff7e4c
Show file tree
Hide file tree
Showing 43 changed files with 172 additions and 149 deletions.
2 changes: 1 addition & 1 deletion attic/manipulation/dev/quasistatic_system.cc
Original file line number Diff line number Diff line change
Expand Up @@ -386,7 +386,7 @@ void QuasistaticSystem<Scalar>::DoCalcWnWfJnJfPhiAnalytic(
static_cast<void>(Jn_ptr);
static_cast<void>(Jf_ptr);
static_cast<void>(phi_ptr);
DRAKE_ABORT_MSG(
throw std::logic_error(
"Analytic expressions for Wn, Wf, Jn, Jf and phi are not "
"available.");
}
Expand Down
6 changes: 4 additions & 2 deletions attic/manipulation/util/robot_state_msg_translator.cc
Original file line number Diff line number Diff line change
Expand Up @@ -164,7 +164,8 @@ void RobotStateLcmMessageTranslator::DecodeMessageKinematics(
v.segment<3>(velocity_start) = R_BJ * V_WB_J.head<3>(); // Rotate.
v.segment<3>(velocity_start + 3) = R_BJ * V_WB_J.tail<3>(); // Translate.
} else {
DRAKE_ABORT_MSG("Floating joint is neither a RPY or a Quaternion joint.");
throw std::domain_error(
"Floating joint is neither a RPY or a Quaternion joint.");
}
}

Expand Down Expand Up @@ -276,7 +277,8 @@ void RobotStateLcmMessageTranslator::EncodeMessageKinematics(
V_JB.head<3>() = R_JB * v.segment<3>(velocity_start);
V_JB.tail<3>() = R_JB * v.segment<3>(velocity_start + 3);
} else {
DRAKE_ABORT_MSG("Floating joint is neither a RPY or a Quaternion joint.");
throw std::domain_error(
"Floating joint is neither a RPY or a Quaternion joint.");
}
} else {
// Fixed base, the transformation is the joint's pose in the world frame.
Expand Down
3 changes: 1 addition & 2 deletions attic/multibody/collision/drake_collision.cc
Original file line number Diff line number Diff line change
Expand Up @@ -31,9 +31,8 @@ unique_ptr<Model> newModel(ModelType type) {
return unique_ptr<Model>(new BulletModel());
}
#endif
default:
DRAKE_ABORT_MSG("Unexpected collision model type.");
}
DRAKE_UNREACHABLE();
}

} // namespace collision
Expand Down
23 changes: 9 additions & 14 deletions attic/multibody/collision/fcl_model.cc
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
#include "drake/multibody/collision/fcl_model.h"

#include <stdexcept>
#include <utility>

#include <Eigen/Dense>
Expand Down Expand Up @@ -98,8 +99,7 @@ void FclModel::DoAddElement(const Element& element) {
std::make_shared<fcl::Cylinderd>(cylinder.radius, cylinder.length);
} break;
default:
DRAKE_ABORT_MSG("Not implemented.");
break;
throw std::logic_error("Not implemented.");
}

auto fcl_object = std::make_unique<fcl::CollisionObjectd>(fcl_geometry);
Expand Down Expand Up @@ -147,8 +147,7 @@ bool FclModel::ClosestPointsAllToAll(
const std::vector<ElementId>& ids_to_check, bool use_margins,
std::vector<PointPair<double>>* closest_points) {
drake::unused(ids_to_check, use_margins, closest_points);
DRAKE_ABORT_MSG("Not implemented.");
return false;
throw std::logic_error("Not implemented.");
}

bool FclModel::ComputeMaximumDepthCollisionPoints(
Expand All @@ -165,45 +164,41 @@ bool FclModel::ClosestPointsPairwise(
const std::vector<ElementIdPair>& id_pairs, bool use_margins,
std::vector<PointPair<double>>* closest_points) {
drake::unused(id_pairs, use_margins, closest_points);
DRAKE_ABORT_MSG("Not implemented.");
return false;
throw std::logic_error("Not implemented.");
}

void FclModel::CollisionDetectFromPoints(
const Eigen::Matrix3Xd& points, bool use_margins,
std::vector<PointPair<double>>* closest_points) {
drake::unused(points, use_margins, closest_points);
DRAKE_ABORT_MSG("Not implemented.");
throw std::logic_error("Not implemented.");
}

void FclModel::ClearCachedResults(bool use_margins) {
drake::unused(use_margins);
DRAKE_ABORT_MSG("Not implemented.");
throw std::logic_error("Not implemented.");
}

bool FclModel::CollisionRaycast(const Eigen::Matrix3Xd& origins,
const Eigen::Matrix3Xd& ray_endpoints,
bool use_margins, Eigen::VectorXd* distances,
Eigen::Matrix3Xd* normals) {
drake::unused(origins, ray_endpoints, use_margins, distances, normals);
DRAKE_ABORT_MSG("Not implemented.");
return false;
throw std::logic_error("Not implemented.");
}

bool FclModel::CollidingPointsCheckOnly(
const std::vector<Eigen::Vector3d>& input_points,
double collision_threshold) {
drake::unused(input_points, collision_threshold);
DRAKE_ABORT_MSG("Not implemented.");
return false;
throw std::logic_error("Not implemented.");
}

std::vector<size_t> FclModel::CollidingPoints(
const std::vector<Eigen::Vector3d>& input_points,
double collision_threshold) {
drake::unused(input_points, collision_threshold);
DRAKE_ABORT_MSG("Not implemented.");
return std::vector<size_t>();
throw std::logic_error("Not implemented.");
}

} // namespace collision
Expand Down
35 changes: 18 additions & 17 deletions attic/multibody/collision/test/model_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -162,11 +162,12 @@ INSTANTIATE_TEST_CASE_P(UsableModelTypesTests, UsableModelTypesTests,

#ifndef DRAKE_DISABLE_FCL
// Fixture for locking down FclModel's not-yet-implemented functions.
class FclModelDeathTests : public ModelTestBase,
public ::testing::WithParamInterface<
std::function<void(FclModelDeathTests*)>> {
class FclModelNotImplementedTests :
public ModelTestBase,
public ::testing::WithParamInterface<
std::function<void(FclModelNotImplementedTests*)>> {
public:
FclModelDeathTests() {
FclModelNotImplementedTests() {
model_ = drake::multibody::collision::newModel(ModelType::kFcl);
}

Expand Down Expand Up @@ -219,22 +220,22 @@ class FclModelDeathTests : public ModelTestBase,
}
};

TEST_P(FclModelDeathTests, NotImplemented) {
::testing::FLAGS_gtest_death_test_style = "threadsafe";
EXPECT_DEATH(GetParam()(this), "Not implemented.");
TEST_P(FclModelNotImplementedTests, NotImplemented) {
EXPECT_THROW(GetParam()(this), std::exception);
}

INSTANTIATE_TEST_CASE_P(
NotImplementedTest, FclModelDeathTests,
::testing::Values(&FclModelDeathTests::CallAddBox,
&FclModelDeathTests::CallAddCapsule,
&FclModelDeathTests::CallAddMesh,
&FclModelDeathTests::CallClosestPointsAllToAll,
&FclModelDeathTests::CallCollisionDetectFromPoints,
&FclModelDeathTests::CallClearCachedResults,
&FclModelDeathTests::CallCollisionRaycast,
&FclModelDeathTests::CallCollidingPointsCheckOnly,
&FclModelDeathTests::CallCollidingPoints));
NotImplementedTest, FclModelNotImplementedTests,
::testing::Values(
&FclModelNotImplementedTests::CallAddBox,
&FclModelNotImplementedTests::CallAddCapsule,
&FclModelNotImplementedTests::CallAddMesh,
&FclModelNotImplementedTests::CallClosestPointsAllToAll,
&FclModelNotImplementedTests::CallCollisionDetectFromPoints,
&FclModelNotImplementedTests::CallClearCachedResults,
&FclModelNotImplementedTests::CallCollisionRaycast,
&FclModelNotImplementedTests::CallCollidingPointsCheckOnly,
&FclModelNotImplementedTests::CallCollidingPoints));
#endif

// Fixture for testing collision queries involving pairs of collision
Expand Down
22 changes: 12 additions & 10 deletions attic/multibody/collision/unusable_model.cc
Original file line number Diff line number Diff line change
@@ -1,54 +1,56 @@
#include "drake/multibody/collision/unusable_model.h"

#include <stdexcept>

#include "drake/common/drake_assert.h"

namespace drake {
namespace multibody {
namespace collision {

void UnusableModel::UpdateModel() {
DRAKE_ABORT_MSG(
throw std::domain_error(
"Compile Drake with a collision library backend for collision support!");
return;
}

bool UnusableModel::UpdateElementWorldTransform(
ElementId, const Eigen::Isometry3d&) {
DRAKE_ABORT_MSG(
throw std::domain_error(
"Compile Drake with a collision library backend for collision support!");
return false;
}

bool UnusableModel::ClosestPointsAllToAll(const std::vector<ElementId>&, bool,
std::vector<PointPair<double>>*) {
DRAKE_ABORT_MSG(
throw std::domain_error(
"Compile Drake with a collision library backend for collision support!");
return false;
}

bool UnusableModel::ComputeMaximumDepthCollisionPoints(
bool, std::vector<PointPair<double>>*) {
DRAKE_ABORT_MSG(
throw std::domain_error(
"Compile Drake with a collision library backend for collision support!");
return false;
}

bool UnusableModel::ClosestPointsPairwise(
const std::vector<ElementIdPair>&, bool, std::vector<PointPair<double>>*) {
DRAKE_ABORT_MSG(
throw std::domain_error(
"Compile Drake with a collision library backend for collision support!");
return false;
}

void UnusableModel::CollisionDetectFromPoints(
const Eigen::Matrix3Xd&, bool, std::vector<PointPair<double>>*) {
DRAKE_ABORT_MSG(
throw std::domain_error(
"Compile Drake with a collision library backend for collision support!");
return;
}

void UnusableModel::ClearCachedResults(bool) {
DRAKE_ABORT_MSG(
throw std::domain_error(
"Compile Drake with a collision library backend for collision support!");
return;
}
Expand All @@ -58,21 +60,21 @@ bool UnusableModel::CollisionRaycast(const Eigen::Matrix3Xd&,
bool,
Eigen::VectorXd*,
Eigen::Matrix3Xd*) {
DRAKE_ABORT_MSG(
throw std::domain_error(
"Compile Drake with a collision library backend for collision support!");
return false;
}

bool UnusableModel::CollidingPointsCheckOnly(
const std::vector<Eigen::Vector3d>&, double) {
DRAKE_ABORT_MSG(
throw std::domain_error(
"Compile Drake with a collision library backend for collision support!");
return false;
}

std::vector<size_t> UnusableModel::CollidingPoints(
const std::vector<Eigen::Vector3d>&, double) {
DRAKE_ABORT_MSG(
throw std::domain_error(
"Compile Drake with a collision library backend for collision support!");
return std::vector<size_t>();
}
Expand Down
3 changes: 2 additions & 1 deletion attic/multibody/rigid_body.h
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#include <map>
#include <memory>
#include <set>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>
Expand Down Expand Up @@ -113,7 +114,7 @@ class RigidBody {
template<typename JointType>
JointType* add_joint(RigidBody* parent, std::unique_ptr<JointType> joint) {
if (joint_ != nullptr) {
DRAKE_ABORT_MSG(
throw std::logic_error(
"Attempting to assign a new joint to a body that already has one");
}
set_parent(parent);
Expand Down
2 changes: 1 addition & 1 deletion attic/multibody/rigid_body_plant/viewer_draw_translator.cc
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ ViewerDrawTranslator::ViewerDrawTranslator(const RigidBodyTree<double>& tree)

void ViewerDrawTranslator::Deserialize(
const void*, int, VectorBase<double>*) const {
DRAKE_ABORT_MSG(
throw std::logic_error(
"The translator that converts from a drake::lcmt_viewer_draw message to "
"a VectorBase object that contains the RigidBodyTree's state vector has not"
"been implemented yet.");
Expand Down
3 changes: 2 additions & 1 deletion attic/systems/sensors/rgbd_renderer_ospray.cc
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#include <fstream>
#include <limits>
#include <map>
#include <stdexcept>
#include <string>
#include <vector>

Expand Down Expand Up @@ -83,7 +84,7 @@ using ActorCollection = std::vector<vtkSmartPointer<vtkActor>>;
std::string RemoveFileExtension(const std::string& filepath) {
const size_t last_dot = filepath.find_last_of(".");
if (last_dot == std::string::npos) {
DRAKE_ABORT_MSG("File has no extension.");
throw std::logic_error("File has no extension.");
}
return filepath.substr(0, last_dot);
}
Expand Down
3 changes: 2 additions & 1 deletion attic/systems/sensors/rgbd_renderer_vtk.cc
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#include <fstream>
#include <limits>
#include <map>
#include <stdexcept>
#include <string>
#include <vector>

Expand Down Expand Up @@ -85,7 +86,7 @@ struct ModuleInitVtkRenderingOpenGL2 {
std::string RemoveFileExtension(const std::string& filepath) {
const size_t last_dot = filepath.find_last_of(".");
if (last_dot == std::string::npos) {
DRAKE_ABORT_MSG("File has no extension.");
throw std::logic_error("File has no extension.");
}
return filepath.substr(0, last_dot);
}
Expand Down
5 changes: 2 additions & 3 deletions automotive/maliput/multilane/builder.h
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
#include <cmath>
#include <map>
#include <memory>
#include <stdexcept>
#include <string>
#include <tuple>
#include <vector>
Expand Down Expand Up @@ -656,14 +657,12 @@ class Builder : public BuilderBase {
case -1: { return true; }
case 1: { return false; }
case 0: { return false; }
default: { DRAKE_ABORT_MSG("fuzzy_compare domain error"); }
}
}
default: { DRAKE_ABORT_MSG("fuzzy_compare domain error"); }
}
}
default: { DRAKE_ABORT_MSG("fuzzy_compare domain error"); }
}
throw std::domain_error("fuzzy_compare domain error");
}

private:
Expand Down
3 changes: 2 additions & 1 deletion automotive/maliput/simplerulebook/simple_rulebook.cc
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#include "drake/automotive/maliput/simplerulebook/simple_rulebook.h"

#include <algorithm>
#include <stdexcept>
#include <string>

#include "drake/common/drake_optional.h"
Expand Down Expand Up @@ -211,7 +212,7 @@ QueryResults SimpleRulebook::DoFindRules(
} else if (id.s) {
result.speed_limit.push_back(speed_limits_.at(*id.s));
} else {
DRAKE_ABORT_MSG("IdVariant is empty");
throw std::domain_error("SimpleRulebook: IdVariant is empty");
}
}
}
Expand Down
4 changes: 3 additions & 1 deletion automotive/maliput_railcar.cc
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
#include <algorithm>
#include <cmath>
#include <limits>
#include <stdexcept>
#include <utility>
#include <vector>

Expand Down Expand Up @@ -449,7 +450,8 @@ void MaliputRailcar<T>::DoCalcUnrestrictedUpdate(
}

if (!next_branch) {
DRAKE_ABORT_MSG("MaliputRailcar::DoCalcUnrestrictedUpdate: ERROR: "
throw std::logic_error(
"MaliputRailcar::DoCalcUnrestrictedUpdate: ERROR: "
"Vehicle should switch lanes but no default or ongoing branch "
"exists.");
} else {
Expand Down
Loading

0 comments on commit bff7e4c

Please sign in to comment.