英文:
MoveIt: How to initialize collision_detection::CollisionEnvFCL with the currently active environment so I can use CollisionEnvFCL::distanceRobot()?
问题
您可以通过以下方式初始化collision_detection::CollisionEnvFCL
环境,将c_env
强制转换为CollisionEnvFCL
类型:
// Get the active collision environment as a CollisionEnvFCL type
const collision_detection::CollisionEnvConstPtr c_env = planning_scene->getCollisionEnv();
collision_detection::CollisionEnvFCL collision_env(c_env->getWorld(), c_env->getRobotModel());
这将使用c_env
中的世界和机器人模型信息来初始化CollisionEnvFCL
。现在,您可以使用collision_env
来执行碰撞检测操作。
希望这对您有所帮助!如果您有任何其他问题,请随时提问。
英文:
I would like to use the collision_detection::CollisionEnvFCL::distanceRobot()
method from MoveIt to compute the minimum distance between a robot and an object (along with some other info that this method provides).
However, I do not know how to properly initialize collision_detection::CollisionEnvFCL with the currently active environment that holds the robot and the object.
This is my attempt so far:
#include <ros/ros.h>
// MoveIt
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/collision_detection_fcl/collision_env_fcl.h>
#include <moveit_msgs/GetPlanningScene.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "panda_coll_detection");
ros::AsyncSpinner spinner(8);
spinner.start();
// ---------------------------------------------------------------------------
// Define interfaces
// ---------------------------------------------------------------------------
// Define move_group_interface & planning_scene_interface
static const std::string PLANNING_GROUP = "panda_arm";
moveit::planning_interface::MoveGroupInterface move_group_interface(PLANNING_GROUP);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
// Define robot model
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel();
// ---------------------------------------------------------------------------
// Add a collision object
// ---------------------------------------------------------------------------
// Define a collision object ROS message for the robot to avoid
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = move_group_interface.getPlanningFrame();
collision_object.id = "box1";
// Define a box to add to the world
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[primitive.BOX_X] = 0.4;
primitive.dimensions[primitive.BOX_Y] = 0.02;
primitive.dimensions[primitive.BOX_Z] = 0.02;
// Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 1.0;//0.5
box_pose.position.y = 0.0;
box_pose.position.z = 0.95; //0.35
// Add box to collision_object
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
// Define a vector of collision objects that could contain additional objects
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);
// Add the collision object into the world
planning_scene_interface.applyCollisionObjects(collision_objects);
// ---------------------------------------------------------------------------
// Get (updated) planning scene
// ---------------------------------------------------------------------------
auto planning_scene = std::make_shared<planning_scene::PlanningScene>(kinematic_model);
ros::NodeHandle h;
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
ros::Duration timeout(1.0);
if (client.waitForExistence(timeout)) {
moveit_msgs::GetPlanningScene::Request req;
moveit_msgs::GetPlanningScene::Response res;
if (client.call(req, res))
planning_scene->setPlanningSceneMsg(res.scene); // apply result to actual PlanningScene
}
// ---------------------------------------------------------------------------
// Collision Checking with CollisionEnvFCL.distanceRobot()
// ---------------------------------------------------------------------------
// Define distance request & result
auto distance_request = collision_detection::DistanceRequest();
auto distance_result = collision_detection::DistanceResult();
distance_request.enable_nearest_points =true;
// Get robot state
moveit::core::RobotState copied_state2 = planning_scene->getCurrentState();
// Get the active collision environment as a CollisionEnvFCL type
// -------- This is where I need help ---------
const collision_detection::CollisionEnvConstPtr c_env = planning_scene->getCollisionEnv();
collision_detection::CollisionEnvFCL collision_env = ???
// --------------------------------------------
// Compute the shortest distance between the robot and the world
collision_env.distanceRobot(distance_request, distance_result, copied_state2);
// Show result
ROS_INFO_STREAM("Current state is " << (distance_result.collision ? "in" : "not in") << " collision");
ROS_INFO_STREAM("Min distance between two bodies=" << distance_result.minimum_distance.distance << "\n");
ros::shutdown();
return 0;
}
So, how can I initialize a collision_detection::CollisionEnvFCL
environment with the currently active environment? Do I need to cast c_env to CollisionEnvFCL and if yes, how?
答案1
得分: 0
以下是翻译好的部分:
如果有其他人感兴趣的话,这是我解决问题的方法:
首先,事实证明活动碰撞环境确实是FCL环境,所以我可以只使用以下代码来执行碰撞检查:
const collision_detection::CollisionEnvConstPtr collision_env = planning_scene->getCollisionEnv();
然而,一般来说,您可以将活动环境更改为FCL环境,如下所示:
#include <moveit/collision_detection_fcl/collision_detector_allocator_fcl.h>
planning_scene->setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create(), true);
ROS_INFO_STREAM("Current active collision env is: " << planning_scene->getActiveCollisionDetectorName());
英文:
In case anyone else is interested, this is how I solved it:
First of all, it turns out that the active collision environment was indeed the FCL environment, so I could perform the collision checking with just this:
const collision_detection::CollisionEnvConstPtr collision_env = planning_scene->getCollisionEnv();
However, in general, you can change the active environment to be the FCL one with:
#include <moveit/collision_detection_fcl/collision_detector_allocator_fcl.h>
planning_scene->setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create(), true);
ROS_INFO_STREAM("Current active collision env is: " << planning_scene->getActiveCollisionDetectorName());
通过集体智慧和协作来改善编程学习和解决问题的方式。致力于成为全球开发者共同参与的知识库,让每个人都能够通过互相帮助和分享经验来进步。
评论