use auto to avoid Affine/Isometry depencency, use namespaced console_bridge

simonschmeisser · simonschmeisser · commit 714f2fbfe70b · 2018-12-19T17:06:20.000+01:00
in melodic MoveIt! switched to using Eigen::Isometry3d instead of Eigen::Affine3d
for performance reasons. Here auto is used to avoid depending on the exact type.

logError and similar were moved into "namespace"/prefix CONSOLE_BRIDGE_ some releases
ago and in melodic the non-prefixed version was removed. The plugin does not seem to
be strictly ros-dependend, therefore I did not use ROS_ERROR
diff --git a/industrial_collision_detection/src/collision_detection/collision_common.cpp b/industrial_collision_detection/src/collision_detection/collision_common.cpp
@@ -78,7 +78,7 @@ namespace collision_detection
         {
           always_allow_collision = true;
           if (!cdata->req->verbose)
-            logDebug("Collision between '%s' and '%s' is always allowed. No contacts are computed.",
+            CONSOLE_BRIDGE_logDebug("Collision between '%s' and '%s' is always allowed. No contacts are computed.",
                      cd1->getID().c_str(), cd2->getID().c_str());
         }
       }
@@ -92,7 +92,7 @@ namespace collision_detection
       {
         always_allow_collision = true;
         if (!cdata->req->verbose)
-          logDebug("Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
+          CONSOLE_BRIDGE_logDebug("Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
                    cd1->getID().c_str(), cd2->getID().c_str());
       }
     }
@@ -105,7 +105,7 @@ namespace collision_detection
         {
           always_allow_collision = true;
           if (!cdata->req->verbose)
-            logDebug("Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
+            CONSOLE_BRIDGE_logDebug("Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
                      cd2->getID().c_str(), cd1->getID().c_str());
         }
       }
@@ -117,7 +117,7 @@ namespace collision_detection
     }
 
     if (!cdata->req->verbose)
-      logDebug("Actually checking collisions between %s and %s", cd1->getID().c_str(), cd2->getID().c_str());
+      CONSOLE_BRIDGE_logDebug("Actually checking collisions between %s and %s", cd1->getID().c_str(), cd2->getID().c_str());
 
 
     fcl::DistanceResult fcl_result;
diff --git a/industrial_collision_detection/src/collision_detection/collision_robot_industrial.cpp b/industrial_collision_detection/src/collision_detection/collision_robot_industrial.cpp
@@ -61,7 +61,7 @@ collision_detection::CollisionRobotIndustrial::CollisionRobotIndustrial(const ro
         fcl_objs_[index] = FCLCollisionObjectConstPtr(new fcl::CollisionObject(g->collision_geometry_));
       }
       else
-        logError("Unable to construct collision geometry for link '%s'", links[i]->getName().c_str());
+        CONSOLE_BRIDGE_logError("Unable to construct collision geometry for link '%s'", links[i]->getName().c_str());
     }
 }
 
@@ -104,7 +104,7 @@ void collision_detection::CollisionRobotIndustrial::constructFCLObject(const rob
   {
     std::vector<FCLGeometryConstPtr> objs;
     getAttachedBodyObjects(ab[j], objs);
-    const EigenSTL::vector_Affine3d &ab_t = ab[j]->getGlobalCollisionBodyTransforms();
+    const auto &ab_t = ab[j]->getGlobalCollisionBodyTransforms();
     for (std::size_t k = 0 ; k < objs.size() ; ++k)
       if (objs[k]->collision_geometry_)
       {
@@ -140,12 +140,12 @@ void collision_detection::CollisionRobotIndustrial::checkSelfCollision(const Col
 
 void collision_detection::CollisionRobotIndustrial::checkSelfCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state1, const robot_state::RobotState &state2) const
 {
-  logError("FCL continuous collision checking not yet implemented");
+  CONSOLE_BRIDGE_logError("FCL continuous collision checking not yet implemented");
 }
 
 void collision_detection::CollisionRobotIndustrial::checkSelfCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state1, const robot_state::RobotState &state2, const AllowedCollisionMatrix &acm) const
 {
-  logError("FCL continuous collision checking not yet implemented");
+  CONSOLE_BRIDGE_logError("FCL continuous collision checking not yet implemented");
 }
 
 void collision_detection::CollisionRobotIndustrial::checkSelfCollisionHelper(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state,
@@ -188,14 +188,14 @@ void collision_detection::CollisionRobotIndustrial::checkOtherCollision(const Co
 void collision_detection::CollisionRobotIndustrial::checkOtherCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state1, const robot_state::RobotState &state2,
                                                                  const CollisionRobot &other_robot, const robot_state::RobotState &other_state1, const robot_state::RobotState &other_state2) const
 {
-  logError("FCL continuous collision checking not yet implemented");
+  CONSOLE_BRIDGE_logError("FCL continuous collision checking not yet implemented");
 }
 
 void collision_detection::CollisionRobotIndustrial::checkOtherCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state1, const robot_state::RobotState &state2,
                                                                  const CollisionRobot &other_robot, const robot_state::RobotState &other_state1, const robot_state::RobotState &other_state2,
                                                                  const AllowedCollisionMatrix &acm) const
 {
-  logError("FCL continuous collision checking not yet implemented");
+  CONSOLE_BRIDGE_logError("FCL continuous collision checking not yet implemented");
 }
 
 void collision_detection::CollisionRobotIndustrial::checkOtherCollisionHelper(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state,
@@ -237,7 +237,7 @@ void collision_detection::CollisionRobotIndustrial::updatedPaddingOrScaling(cons
       }
     }
     else
-      logError("Updating padding or scaling for unknown link: '%s'", links[i].c_str());
+      CONSOLE_BRIDGE_logError("Updating padding or scaling for unknown link: '%s'", links[i].c_str());
   }
 }
 
@@ -331,7 +331,7 @@ void collision_detection::CollisionRobotIndustrial::distanceOther(const Distance
   catch(const std::bad_cast& e)
   {
     std::string msg = "Dynamic casting to CollisionRobotIndustrial failed";
-    logError("%s",msg.c_str());
+    CONSOLE_BRIDGE_logError("%s",msg.c_str());
     throw std::runtime_error(msg);
   }
 }
diff --git a/industrial_collision_detection/src/collision_detection/collision_world_industrial.cpp b/industrial_collision_detection/src/collision_detection/collision_world_industrial.cpp
@@ -98,12 +98,12 @@ void collision_detection::CollisionWorldIndustrial::checkRobotCollision(const Co
 
 void collision_detection::CollisionWorldIndustrial::checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state1, const robot_state::RobotState &state2) const
 {
-  logError("FCL continuous collision checking not yet implemented");
+  CONSOLE_BRIDGE_logError("FCL continuous collision checking not yet implemented");
 }
 
 void collision_detection::CollisionWorldIndustrial::checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state1, const robot_state::RobotState &state2, const AllowedCollisionMatrix &acm) const
 {
-  logError("FCL continuous collision checking not yet implemented");
+  CONSOLE_BRIDGE_logError("FCL continuous collision checking not yet implemented");
 }
 
 void collision_detection::CollisionWorldIndustrial::checkRobotCollisionHelper(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix *acm) const

Original file line number	Diff line number	Diff line change
`@@ -78,7 +78,7 @@ namespace collision_detection`
`78`	`78`	`{`
`79`	`79`	`always_allow_collision = true;`
`80`	`80`	`if (!cdata->req->verbose)`
`81`		`- logDebug("Collision between '%s' and '%s' is always allowed. No contacts are computed.",`
	`81`	`+ CONSOLE_BRIDGE_logDebug("Collision between '%s' and '%s' is always allowed. No contacts are computed.",`
`82`	`82`	`cd1->getID().c_str(), cd2->getID().c_str());`
`83`	`83`	`}`
`84`	`84`	`}`
`@@ -92,7 +92,7 @@ namespace collision_detection`
`92`	`92`	`{`
`93`	`93`	`always_allow_collision = true;`
`94`	`94`	`if (!cdata->req->verbose)`
`95`		`- logDebug("Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",`
	`95`	`+ CONSOLE_BRIDGE_logDebug("Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",`
`96`	`96`	`cd1->getID().c_str(), cd2->getID().c_str());`
`97`	`97`	`}`
`98`	`98`	`}`
`@@ -105,7 +105,7 @@ namespace collision_detection`
`105`	`105`	`{`
`106`	`106`	`always_allow_collision = true;`
`107`	`107`	`if (!cdata->req->verbose)`
`108`		`- logDebug("Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",`
	`108`	`+ CONSOLE_BRIDGE_logDebug("Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",`
`109`	`109`	`cd2->getID().c_str(), cd1->getID().c_str());`
`110`	`110`	`}`
`111`	`111`	`}`
`@@ -117,7 +117,7 @@ namespace collision_detection`
`117`	`117`	`}`
`118`	`118`
`119`	`119`	`if (!cdata->req->verbose)`
`120`		`- logDebug("Actually checking collisions between %s and %s", cd1->getID().c_str(), cd2->getID().c_str());`
	`120`	`+ CONSOLE_BRIDGE_logDebug("Actually checking collisions between %s and %s", cd1->getID().c_str(), cd2->getID().c_str());`
`121`	`121`
`122`	`122`
`123`	`123`	`fcl::DistanceResult fcl_result;`
Original file line number	Diff line number	Diff line change
`@@ -61,7 +61,7 @@ collision_detection::CollisionRobotIndustrial::CollisionRobotIndustrial(const ro`
`61`	`61`	`fcl_objs_[index] = FCLCollisionObjectConstPtr(new fcl::CollisionObject(g->collision_geometry_));`
`62`	`62`	`}`
`63`	`63`	`else`
`64`		`- logError("Unable to construct collision geometry for link '%s'", links[i]->getName().c_str());`
	`64`	`+ CONSOLE_BRIDGE_logError("Unable to construct collision geometry for link '%s'", links[i]->getName().c_str());`
`65`	`65`	`}`
`66`	`66`	`}`
`67`	`67`
`@@ -104,7 +104,7 @@ void collision_detection::CollisionRobotIndustrial::constructFCLObject(const rob`
`104`	`104`	`{`
`105`	`105`	`std::vector<FCLGeometryConstPtr> objs;`
`106`	`106`	`getAttachedBodyObjects(ab[j], objs);`
`107`		`- const EigenSTL::vector_Affine3d &ab_t = ab[j]->getGlobalCollisionBodyTransforms();`
	`107`	`+ const auto &ab_t = ab[j]->getGlobalCollisionBodyTransforms();`
`108`	`108`	`for (std::size_t k = 0 ; k < objs.size() ; ++k)`
`109`	`109`	`if (objs[k]->collision_geometry_)`
`110`	`110`	`{`
`@@ -140,12 +140,12 @@ void collision_detection::CollisionRobotIndustrial::checkSelfCollision(const Col`
`140`	`140`
`141`	`141`	`void collision_detection::CollisionRobotIndustrial::checkSelfCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state1, const robot_state::RobotState &state2) const`
`142`	`142`	`{`
`143`		`- logError("FCL continuous collision checking not yet implemented");`
	`143`	`+ CONSOLE_BRIDGE_logError("FCL continuous collision checking not yet implemented");`
`144`	`144`	`}`
`145`	`145`
`146`	`146`	`void collision_detection::CollisionRobotIndustrial::checkSelfCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state1, const robot_state::RobotState &state2, const AllowedCollisionMatrix &acm) const`
`147`	`147`	`{`
`148`		`- logError("FCL continuous collision checking not yet implemented");`
	`148`	`+ CONSOLE_BRIDGE_logError("FCL continuous collision checking not yet implemented");`
`149`	`149`	`}`
`150`	`150`
`151`	`151`	`void collision_detection::CollisionRobotIndustrial::checkSelfCollisionHelper(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state,`
`@@ -188,14 +188,14 @@ void collision_detection::CollisionRobotIndustrial::checkOtherCollision(const Co`
`188`	`188`	`void collision_detection::CollisionRobotIndustrial::checkOtherCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state1, const robot_state::RobotState &state2,`
`189`	`189`	`const CollisionRobot &other_robot, const robot_state::RobotState &other_state1, const robot_state::RobotState &other_state2) const`
`190`	`190`	`{`
`191`		`- logError("FCL continuous collision checking not yet implemented");`
	`191`	`+ CONSOLE_BRIDGE_logError("FCL continuous collision checking not yet implemented");`
`192`	`192`	`}`
`193`	`193`
`194`	`194`	`void collision_detection::CollisionRobotIndustrial::checkOtherCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state1, const robot_state::RobotState &state2,`
`195`	`195`	`const CollisionRobot &other_robot, const robot_state::RobotState &other_state1, const robot_state::RobotState &other_state2,`
`196`	`196`	`const AllowedCollisionMatrix &acm) const`
`197`	`197`	`{`
`198`		`- logError("FCL continuous collision checking not yet implemented");`
	`198`	`+ CONSOLE_BRIDGE_logError("FCL continuous collision checking not yet implemented");`
`199`	`199`	`}`
`200`	`200`
`201`	`201`	`void collision_detection::CollisionRobotIndustrial::checkOtherCollisionHelper(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state,`
`@@ -237,7 +237,7 @@ void collision_detection::CollisionRobotIndustrial::updatedPaddingOrScaling(cons`
`237`	`237`	`}`
`238`	`238`	`}`
`239`	`239`	`else`
`240`		`- logError("Updating padding or scaling for unknown link: '%s'", links[i].c_str());`
	`240`	`+ CONSOLE_BRIDGE_logError("Updating padding or scaling for unknown link: '%s'", links[i].c_str());`
`241`	`241`	`}`
`242`	`242`	`}`
`243`	`243`
`@@ -331,7 +331,7 @@ void collision_detection::CollisionRobotIndustrial::distanceOther(const Distance`
`331`	`331`	`catch(const std::bad_cast& e)`
`332`	`332`	`{`
`333`	`333`	`std::string msg = "Dynamic casting to CollisionRobotIndustrial failed";`
`334`		`- logError("%s",msg.c_str());`
	`334`	`+ CONSOLE_BRIDGE_logError("%s",msg.c_str());`
`335`	`335`	`throw std::runtime_error(msg);`
`336`	`336`	`}`
`337`	`337`	`}`
Original file line number	Diff line number	Diff line change
`@@ -98,12 +98,12 @@ void collision_detection::CollisionWorldIndustrial::checkRobotCollision(const Co`
`98`	`98`
`99`	`99`	`void collision_detection::CollisionWorldIndustrial::checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state1, const robot_state::RobotState &state2) const`
`100`	`100`	`{`
`101`		`- logError("FCL continuous collision checking not yet implemented");`
	`101`	`+ CONSOLE_BRIDGE_logError("FCL continuous collision checking not yet implemented");`
`102`	`102`	`}`
`103`	`103`
`104`	`104`	`void collision_detection::CollisionWorldIndustrial::checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state1, const robot_state::RobotState &state2, const AllowedCollisionMatrix &acm) const`
`105`	`105`	`{`
`106`		`- logError("FCL continuous collision checking not yet implemented");`
	`106`	`+ CONSOLE_BRIDGE_logError("FCL continuous collision checking not yet implemented");`
`107`	`107`	`}`
`108`	`108`
`109`	`109`	`void collision_detection::CollisionWorldIndustrial::checkRobotCollisionHelper(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix *acm) const`