From e237c435c9593bd6209c6930f36b7c516ef9f958 Mon Sep 17 00:00:00 2001 From: Edgar Date: Wed, 6 Mar 2024 16:56:26 -0500 Subject: [PATCH] Example on how to read and update poses of obstacles in prx::world_model_t --- task_planning/CMakeLists.txt | 1 + .../launch/obstacles_to_ml4kp.launch | 11 ++ .../src/obstacles_to_ml4kp_world.cpp | 129 ++++++++++++++++++ 3 files changed, 141 insertions(+) create mode 100644 task_planning/launch/obstacles_to_ml4kp.launch create mode 100644 task_planning/src/obstacles_to_ml4kp_world.cpp diff --git a/task_planning/CMakeLists.txt b/task_planning/CMakeLists.txt index 8e959a31..9c2f8708 100644 --- a/task_planning/CMakeLists.txt +++ b/task_planning/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS roslib motion_planning utils + tf ) ## System dependencies are found with CMake's conventions diff --git a/task_planning/launch/obstacles_to_ml4kp.launch b/task_planning/launch/obstacles_to_ml4kp.launch new file mode 100644 index 00000000..af9f466b --- /dev/null +++ b/task_planning/launch/obstacles_to_ml4kp.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/task_planning/src/obstacles_to_ml4kp_world.cpp b/task_planning/src/obstacles_to_ml4kp_world.cpp new file mode 100644 index 00000000..1a1a9e20 --- /dev/null +++ b/task_planning/src/obstacles_to_ml4kp_world.cpp @@ -0,0 +1,129 @@ +#include + +#include +#include "prx_models/MushrPlanner.h" +#include "prx_models/mj_mushr.hpp" +#include "motion_planning/replanner_service.hpp" +#include "motion_planning/planner_client.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +tf2_ros::Buffer tf_buffer; +bool get_obstacle_pose(const std::string& world_frame, const std::string& obstacle_frame, + geometry_msgs::TransformStamped& tf) +{ + try + { + tf = tf_buffer.lookupTransform(world_frame, obstacle_frame, ros::Time(0)); + + return true; + } + catch (tf2::TransformException& ex) + { + } + return false; +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "obstacles_to_ml4kp_world"); + ros::NodeHandle nh("~"); + prx::simulation_step = 0.01; + + const std::string root{ ros::this_node::getNamespace() }; + prx::init_random(112392); + + tf2_ros::TransformListener tf_listener(tf_buffer); + + std::vector obstacle_frames; + std::string world_frame, robot_frame; + ROS_PARAM_SETUP(nh, obstacle_frames); + ROS_PARAM_SETUP(nh, world_frame); + ROS_PARAM_SETUP(nh, robot_frame); + + const std::string plant_name{ "mushr" }; + const std::string plant_path{ "mushr" }; + auto plant = prx::system_factory_t::create_system(plant_name, plant_path); + prx_assert(plant != nullptr, "Failed to create plant"); + + prx::world_model_t planning_model({ plant }, {}); + const std::string context_name{ "planner_context" }; + planning_model.create_context(context_name, { plant_name }, {}); + auto planning_context = planning_model.get_context(context_name); + auto ss = planning_context.first->get_state_space(); + auto cs = planning_context.first->get_control_space(); + std::vector min_control_limits = { -1., -0.5 }; + std::vector max_control_limits = { 1., 0.5 }; + cs->set_bounds(min_control_limits, max_control_limits); + + ros::Rate rate(30); + geometry_msgs::TransformStamped tf; + + bool pose_received{ false }; + // Eigen::Vector3d box_dims{ 0.32, 1.0, 0.28 }; + Eigen::Vector3d box_dims{ 0.41, 0.32, 0.28 }; + + std::unordered_set obstacle_set{}; + + std::ofstream ofs_obstacles; // ("/tmp/ml4kp_world_obstacles.txt", std::ofstream::trunc); + std::size_t iters{ 0 }; + prx::trajectory_t traj{ ss }; + + while (ros::ok()) + { + ofs_obstacles = std::ofstream("/tmp/ml4kp_world_obstacles.txt", std::ofstream::trunc); + for (auto obstacle : obstacle_frames) + { + if (get_obstacle_pose(world_frame, obstacle, tf)) + { + prx::transform_t pose{ tf2::transformToEigen(tf) }; + // pose.translation()[2] *= 2; // Make Z double, as ml4kp considers the center and this returns the top + pose.translation()[2] = box_dims[2] / 2; // Make Z half the height of box to "Zero" it + + if (obstacle_set.count(obstacle) == 0) + { + planning_model.emplace_obstacle(context_name, obstacle, box_dims[0], box_dims[1], box_dims[2], + pose); + obstacle_set.insert(obstacle); + } + else + { + const std::shared_ptr box_movable{ planning_model.obstacle(obstacle) }; + std::shared_ptr box_tf{ box_movable->transform_ptr("body") }; + *box_tf = pose; // Move it out so there is no collision + // DEBUG_VARS(obstacle, pose.translation().transpose()); + } + } + // ofs_obstacles << pose.translation()[0]; + } + if (get_obstacle_pose(world_frame, robot_frame, tf)) + { + prx_models::copy(*(ss), tf); + traj.copy_onto_back(ss); + // DEBUG_VARS(robot_frame, *ss); + } + rate.sleep(); + } + + auto obstacles = planning_model.get_obstacles(); + prx::three_js_group_t* vis_group = new prx::three_js_group_t({ plant }, { obstacles }); + prx::space_point_t default_state{ ss->make_point() }; + ss->copy(default_state, Eigen::Vector4d::Zero()); + + vis_group->add_animation(traj, ss, default_state); + + // std::string body_name = plant_name + "/" + "body"; + + vis_group->output_html("ros_ml4kp.html"); + + return 0; +} \ No newline at end of file