diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 1ae5f6a..8c7c24e 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -26,6 +26,7 @@ RUN apt update && \ apt install -y \ build-essential \ clang-format-14 \ + clangd-12 \ cmake \ git-lfs \ python3-colcon-common-extensions \ @@ -36,7 +37,8 @@ RUN apt update && \ ros-dev-tools \ ros-${ROS_DISTRO}-desktop \ ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \ - wget + wget && \ + update-alternatives --install /usr/bin/clangd clangd /usr/bin/clangd-12 100 RUN pip3 install colcon-mixin pre-commit && \ colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml && \ diff --git a/docker-compose.yaml b/docker-compose.yaml index 228b146..237c242 100644 --- a/docker-compose.yaml +++ b/docker-compose.yaml @@ -1,5 +1,5 @@ services: - moveit: + moveit_drake: image: moveit_drake:latest container_name: moveit_drake build: diff --git a/include/ktopt_interface/ktopt_planning_context.hpp b/include/ktopt_interface/ktopt_planning_context.hpp index 547f442..4fc0060 100644 --- a/include/ktopt_interface/ktopt_planning_context.hpp +++ b/include/ktopt_interface/ktopt_planning_context.hpp @@ -29,6 +29,7 @@ namespace ktopt_interface // declare all namespaces to be used using drake::geometry::AddRigidHydroelasticProperties; using drake::geometry::Box; +using drake::geometry::Cylinder; using drake::geometry::FrameId; using drake::geometry::GeometryFrame; using drake::geometry::GeometryId; @@ -43,6 +44,7 @@ using drake::geometry::ProximityProperties; using drake::geometry::Role; using drake::geometry::SceneGraph; using drake::geometry::SourceId; +using drake::geometry::Sphere; using drake::math::RigidTransformd; using drake::multibody::AddMultibodyPlantSceneGraph; using drake::multibody::MinimumDistanceLowerBoundConstraint; @@ -56,6 +58,7 @@ using drake::systems::Diagram; using drake::systems::DiagramBuilder; using drake::visualization::ApplyVisualizationConfig; using drake::visualization::VisualizationConfig; +using Eigen::Matrix3d; using Eigen::MatrixXd; using Eigen::Vector3d; using Eigen::VectorXd; diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp index 3f38d1b..11d8a17 100644 --- a/src/ktopt_planning_context.cpp +++ b/src/ktopt_planning_context.cpp @@ -160,7 +160,7 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) visualizer_->ForcedPublish(vis_context); // Without these sleeps, the visualizer won't give you time to load your browser - std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(params_.trajectory_time_step * 1000.0))); + std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(params_.trajectory_time_step * 10000.0))); } visualizer_->StopRecording(); visualizer_->PublishRecording(); @@ -228,6 +228,7 @@ void KTOptPlanningContext::setRobotDescription(std::string robot_description) void KTOptPlanningContext::transcribePlanningScene(const planning_scene::PlanningScene& planning_scene) { + // Transcribe the planning scene into a drake scene graph try { auto world = planning_scene.getWorld(); @@ -248,29 +249,83 @@ void KTOptPlanningContext::transcribePlanningScene(const planning_scene::Plannin } if (object == OCTOMAP_NS) { - RCLCPP_ERROR(getLogger(), "skipping octomap for now ..."); + RCLCPP_WARN(getLogger(), "Octomap not supported for now ... "); continue; } - for (const auto& shape : collision_object->shapes_) + for (int i = 0; i < collision_object->shapes_.size(); ++i) { - RCLCPP_ERROR(getLogger(), "iterating inside collision object's shapes"); - const auto& pose = collision_object->shape_poses_[0]; + std::string shape_name = object + std::to_string(i); + const auto& shape = collision_object->shapes_[i]; + const auto& pose = collision_object->pose_; + const auto& shape_pose = collision_object->shape_poses_[i]; + const auto& shape_type = collision_object->shapes_[i]->type; + + switch (shape_type) + { + case shapes::ShapeType::BOX: + { + const auto objectptr = std::dynamic_pointer_cast(shape); + const SourceId box_source_id = scene_graph.RegisterSource(shape_name); + const GeometryId box_geom_id = scene_graph.RegisterAnchoredGeometry( + box_source_id, + std::make_unique( + RigidTransformd(pose), + std::make_unique(objectptr->size[0], objectptr->size[1], objectptr->size[2]), shape_name)); + + // add illustration, proximity, perception properties + scene_graph.AssignRole(box_source_id, box_geom_id, IllustrationProperties()); + scene_graph.AssignRole(box_source_id, box_geom_id, ProximityProperties()); + scene_graph.AssignRole(box_source_id, box_geom_id, PerceptionProperties()); + break; + } + case shapes::ShapeType::UNKNOWN_SHAPE: + { + RCLCPP_WARN(getLogger(), "Unknown shape, ignoring in scene graph"); + break; + } + case shapes::ShapeType::SPHERE: + { + const auto objectptr = std::dynamic_pointer_cast(shape); + const SourceId box_source_id = scene_graph.RegisterSource(shape_name); + const GeometryId box_geom_id = scene_graph.RegisterAnchoredGeometry( + box_source_id, std::make_unique( + RigidTransformd(pose), std::make_unique(objectptr->radius), shape_name)); + RCLCPP_INFO(getLogger(), "Sphere"); + + // add illustration, proximity, perception properties + scene_graph.AssignRole(box_source_id, box_geom_id, IllustrationProperties()); + scene_graph.AssignRole(box_source_id, box_geom_id, ProximityProperties()); + scene_graph.AssignRole(box_source_id, box_geom_id, PerceptionProperties()); + break; + } + case shapes::ShapeType::CYLINDER: + { + const auto objectptr = std::dynamic_pointer_cast(shape); + const SourceId box_source_id = scene_graph.RegisterSource(shape_name); + const GeometryId box_geom_id = scene_graph.RegisterAnchoredGeometry( + box_source_id, + std::make_unique( + RigidTransformd(pose), std::make_unique(objectptr->radius, objectptr->length), shape_name)); + RCLCPP_INFO(getLogger(), "Cylinder"); + + // add illustration, proximity, perception properties + scene_graph.AssignRole(box_source_id, box_geom_id, IllustrationProperties()); + scene_graph.AssignRole(box_source_id, box_geom_id, ProximityProperties()); + scene_graph.AssignRole(box_source_id, box_geom_id, PerceptionProperties()); + break; + } + case shapes::ShapeType::CONE: + { + RCLCPP_WARN(getLogger(), "Cone not supported in drake"); + break; + } + default: + { + RCLCPP_WARN(getLogger(), "Shape TYPE conversation to drake is not implemented"); + } + } - // Creates a box geometry and anchors it to the world origin. Better - // approach is to create a ground object, anchor that, and then anchor - // every non-moving entity to the ground plane // TODO: Create and anchor ground entity - Vector3d p(0.3, -0.3, 0.5); - const SourceId box_source_id = scene_graph.RegisterSource("box1"); - const GeometryId box_geom_id = scene_graph.RegisterAnchoredGeometry( - box_source_id, std::make_unique( - RigidTransformd(p), std::make_unique(0.15, 0.15, 0.15), - "box")); // hard coded for now because I know box dimensions and pose, from - - // add illustration, proximity, perception properties - scene_graph.AssignRole(box_source_id, box_geom_id, IllustrationProperties()); - scene_graph.AssignRole(box_source_id, box_geom_id, ProximityProperties()); - scene_graph.AssignRole(box_source_id, box_geom_id, PerceptionProperties()); } } }