Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

More collision scenes #32

Merged
merged 5 commits into from
Aug 14, 2024
Merged

More collision scenes #32

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
eeee39a
WIP
sjahr Aug 9, 2024
8726634
More examples
sjahr Aug 12, 2024
acac6aa
Remove can collision objects
sjahr Aug 13, 2024
cb628b4
Update demo/src/pipeline_testbench_main.cpp
sjahr Aug 13, 2024
a29d4a5
Format
sjahr Aug 13, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion demo/launch/pipeline_testbench.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ def launch_setup(context, *args, **kwargs):
sqlite_database = os.path.join(
get_package_share_directory("moveit_benchmark_resources"),
"databases",
"panda_kitchen_test_db.sqlite",
"panda_benchmarks.sqlite",
)

warehouse_ros_config = {
Expand Down
118 changes: 78 additions & 40 deletions demo/src/pipeline_testbench_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,37 @@ namespace
{
const rclcpp::Logger LOGGER = rclcpp::get_logger("pipeline_testbench");
const std::string PLANNING_GROUP = "panda_arm";
static const std::vector<std::string> CONTROLLERS(1, "panda_arm_controller");
const std::vector<std::string> CONTROLLERS(1, "panda_arm_controller");
const std::vector<std::string> SENSED_SCENE_NAMES = {
"bookshelf_small_panda//scene_sensed0001.yaml", "bookshelf_tall_panda//scene_sensed0001.yaml",
"bookshelf_thin_panda//scene_sensed0001.yaml", "cage_panda//scene_sensed0001.yaml",
"kitchen_panda//scene_sensed0001.yaml", "table_bars_panda//scene_sensed0001.yaml",
"table_pick_panda//scene_sensed0001.yaml", "table_under_pick_panda//scene_sensed0001.yaml"
};

const std::vector<std::string> SCENE_NAMES = {
"bookshelf_small_panda//scene0001.yaml", "bookshelf_tall_panda//scene0001.yaml",
"bookshelf_thin_panda//scene0001.yaml", "cage_panda//scene0001.yaml",
"kitchen_panda//scene0001.yaml", "table_bars_panda//scene0001.yaml",
"table_pick_panda//scene0001.yaml", "table_under_pick_panda//scene0001.yaml"
};

const std::vector<std::string> ALL_SCENE_NAMES = { "bookshelf_small_panda//scene0001.yaml",
"bookshelf_small_panda//scene_sensed0001.yaml",
"bookshelf_tall_panda//scene0001.yaml",
"bookshelf_tall_panda//scene_sensed0001.yaml",
"bookshelf_thin_panda//scene0001.yaml",
"bookshelf_thin_panda//scene_sensed0001.yaml",
"cage_panda//scene0001.yaml",
"cage_panda//scene_sensed0001.yaml",
"kitchen_panda//scene0001.yaml",
"kitchen_panda//scene_sensed0001.yaml",
"table_bars_panda//scene0001.yaml",
"table_bars_panda//scene_sensed0001.yaml",
"table_pick_panda//scene0001.yaml",
"table_pick_panda//scene_sensed0001.yaml",
"table_under_pick_panda//scene0001.yaml"
"table_under_pick_panda//scene_sensed0001.yaml" };
} // namespace
namespace pipeline_testbench
{
Expand Down Expand Up @@ -97,37 +127,29 @@ class Demo
{
moveit_cpp_->getPlanningSceneMonitorNonConst()->providePlanningSceneService();

{
planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_->getPlanningSceneMonitorNonConst());
scene->getCurrentStateNonConst().setVariablePosition("panda_finger_joint1", 0.04);
scene->getCurrentStateNonConst().setVariablePosition("panda_finger_joint2", 0.04);
}

visual_tools_.deleteAllMarkers();
visual_tools_.loadRemoteControl();

Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools_.publishText(text_pose, "Pipeline Testbench", rvt::WHITE, rvt::XLARGE);
visual_tools_.trigger();

// TODO: temporary change to planning scene, adds a box replace once
// integration tests are implemented
geometry_msgs::msg::Pose block_pose;
block_pose.position.z = 0.5;
block_pose.position.y = -0.3;
block_pose.position.x = 0.3;
visual_tools_.publishCollisionBlock(block_pose, "test_block", 0.15);
}

bool loadPlanningSceneAndQuery()
bool loadPlanningSceneAndQuery(const std::string& scene_name, std::string query_name = "Motion Plan Request 0")
{
std::string hostname = "";
int port = 0.0;
std::string scene_name = "";

node_->get_parameter_or(std::string("warehouse.host"), hostname, std::string("127.0.0.1"));
node_->get_parameter_or(std::string("warehouse.port"), port, 33829);

if (!node_->get_parameter("warehouse.scene_name", scene_name))
{
RCLCPP_WARN(LOGGER, "Warehouse scene_name NOT specified");
}

moveit_warehouse::PlanningSceneStorage* planning_scene_storage = nullptr;

// Initialize database connection
Expand Down Expand Up @@ -189,27 +211,38 @@ class Demo
// Add object to planning scene
{ // Lock PlanningScene
planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_->getPlanningSceneMonitorNonConst());
scene->removeAllCollisionObjects();
scene->processPlanningSceneWorldMsg(scene_msg.world);

// Remove collision objects containing "Can" from the scene so that more motions are feasible
const std::vector<std::string>& object_ids = scene->getWorld()->getObjectIds();
for (const std::string& object_id : object_ids)
{
if (object_id.find("Can") != std::string::npos)
{
moveit_msgs::msg::CollisionObject object_to_remove;
object_to_remove.id = object_id;
object_to_remove.operation = moveit_msgs::msg::CollisionObject::REMOVE;
scene->processCollisionObjectMsg(object_to_remove);
}
}
} // Unlock PlanningScene

RCLCPP_INFO(LOGGER, "Loaded planning scene successfully");

// Get planning scene query
for (int index = 0; index < 10; index++)
moveit_warehouse::MotionPlanRequestWithMetadata planning_query;
try
{
moveit_warehouse::MotionPlanRequestWithMetadata planning_query;
std::string query_name = "kitchen_panda_scene_sensed" + std::to_string(index) + "_query";
try
{
planning_scene_storage->getPlanningQuery(planning_query, scene_name, query_name);
}
catch (std::exception& ex)
{
RCLCPP_ERROR(LOGGER, "Error loading motion planning query '%s': %s", query_name.c_str(), ex.what());
}

motion_plan_requests.push_back(static_cast<moveit_msgs::msg::MotionPlanRequest>(*planning_query));
planning_scene_storage->getPlanningQuery(planning_query, scene_name, query_name);
}
catch (std::exception& ex)
{
RCLCPP_ERROR(LOGGER, "Error loading motion planning query '%s': %s", query_name.c_str(), ex.what());
}
motion_plan_requests.clear();
motion_plan_requests.push_back(static_cast<moveit_msgs::msg::MotionPlanRequest>(*planning_query));

visual_tools_.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
visual_tools_.trigger();
return true;
Expand Down Expand Up @@ -322,21 +355,26 @@ int main(int argc, char** argv)

pipeline_testbench::Demo demo(node);

if (!demo.loadPlanningSceneAndQuery())
for (const auto& scene_name : ALL_SCENE_NAMES)
{
rclcpp::shutdown();
return 0;
}
if (!demo.loadPlanningSceneAndQuery(scene_name))
{
rclcpp::shutdown();
return 0;
}

RCLCPP_INFO(LOGGER, "Starting Pipeline Testbench example ...");
for (const auto& motion_plan_req : demo.getMotionPlanRequests())
{
demo.planAndVisualize(
{ { "ompl", "RRTConnectkConfigDefault" }, { "stomp", "stomp" }, { "drake_ktopt", "" }, { "drake_toppra", "" } },
motion_plan_req);
RCLCPP_INFO(LOGGER, "Starting Pipeline Testbench example ...");
for (const auto& motion_plan_req : demo.getMotionPlanRequests())
{
demo.planAndVisualize({ { "ompl", "RRTConnectkConfigDefault" },
{ "stomp", "stomp" },
{ "drake_ktopt", "" },
{ "drake_toppra", "" } },
motion_plan_req);
}
}

demo.getVisualTools().prompt("Press 'next' in the RvizVisualToolsGui window to finish the demo");

rclcpp::shutdown();
return 0;
}
4 changes: 2 additions & 2 deletions moveit_drake.repos
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,5 +21,5 @@ repositories:
version: ros2
moveit_benchmark_resources:
type: git
url: https://github.com/moveit/moveit_benchmark_resources
version: main
url: https://github.com/sjahr/moveit_benchmark_resources
version: pr-update-database-usage
Loading