Skip to content

Commit

Permalink
feedback updates
Browse files Browse the repository at this point in the history
  • Loading branch information
@peterdavidfagan
peterdavidfagan committed Sep 16, 2022
1 parent b2ed79c commit 88110be
Show file tree
Hide file tree
Showing 6 changed files with 292 additions and 76 deletions.
229 changes: 173 additions & 56 deletions python/moveit_py/src/moveit_py/core.cpp
View file

Large diffs are not rendered by default.

76 changes: 63 additions & 13 deletions python/moveit_py/src/moveit_py/moveit_core/planning_scene/planning_scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,10 +44,48 @@ namespace moveit_py
namespace bind_planning_scene
{

void apply_collision_object(std::shared_ptr<planning_scene::PlanningScene>& planning_scene, py::object& collision_object)
void apply_planning_scene_world(std::shared_ptr<planning_scene::PlanningScene>& planning_scene,
py::object& planning_scene_world_msg)
{
moveit_msgs::msg::CollisionObject collision_object_cpp = CollisionObjectToCpp(collision_object);
planning_scene->processCollisionObjectMsg(collision_object_cpp);
moveit_msgs::msg::PlanningSceneWorld planning_scene_world_cpp;
py::module_ rclpy_serialization = py::module::import("rclpy.serialization");
py::bytes serialized_msg = rclpy_serialization.attr("serialize_message")(planning_scene_world_msg);
deserializeMsg(serialized_msg, planning_scene_world_cpp);
planning_scene->processPlanningSceneWorldMsg(planning_scene_world_cpp);
}

// void apply_collision_object(std::shared_ptr<planning_scene::PlanningScene>& planning_scene, py::object& collision_object)
//{
// moveit_msgs::msg::CollisionObject collision_object_cpp = CollisionObjectToCpp(collision_object);
// planning_scene->processCollisionObjectMsg(collision_object_cpp);
// }

void apply_collision_object(std::shared_ptr<planning_scene::PlanningScene>& planning_scene,
py::object& collision_object_msg, std::optional<py::object>& color_msg)
{
// convert collision object message to cpp equivalent
moveit_msgs::msg::CollisionObject collision_object_cpp = CollisionObjectToCpp(collision_object_msg);

// check if color message is provided
if (color_msg.has_value())
{
// convert color message to cpp equivalent (serialization method used for now)
moveit_msgs::msg::ObjectColor color_cpp;
py::module_ rclpy_serialization = py::module::import("rclpy.serialization");
py::bytes serialized_msg = rclpy_serialization.attr("serialize_message")(color_msg);
deserializeMsg(serialized_msg, color_cpp);

// apply collision object
planning_scene->processCollisionObjectMsg(collision_object_cpp);

// set object color
planning_scene->setObjectColor(color_cpp.id, color_cpp.color);
}
else
{
// apply collision object
planning_scene->processCollisionObjectMsg(collision_object_cpp);
}
}

void apply_attached_collision_object(std::shared_ptr<planning_scene::PlanningScene>& planning_scene,
Expand Down Expand Up @@ -76,6 +114,18 @@ Eigen::MatrixXd get_frame_transform(std::shared_ptr<planning_scene::PlanningScen
return transformation.matrix();
}

py::object get_planning_scene_msg(std::shared_ptr<planning_scene::PlanningScene>& planning_scene)
{
moveit_msgs::msg::PlanningScene planning_scene_msg;
planning_scene->getPlanningSceneMsg(planning_scene_msg);
py::module_ rclpy_serialization = py::module::import("rclpy.serialization");
py::bytes serialized_msg = serializeMsg(planning_scene_msg);

py::module_ moveit_msgs_planning_scene = py::module::import("moveit_msgs.msg._planning_scene");
py::object planning_scene_msg_py = moveit_msgs_planning_scene.attr("PlanningScene")();
return rclpy_serialization.attr("deserialize_message")(serialized_msg, planning_scene_msg_py);
}

bool is_path_valid(std::shared_ptr<planning_scene::PlanningScene>& planning_scene,
robot_trajectory::RobotTrajectory& robot_trajectory, std::string& group, bool verbose)
{
Expand Down Expand Up @@ -105,16 +155,16 @@ moveit::core::RobotState& get_current_state(std::shared_ptr<planning_scene::Plan
return planning_scene->getCurrentStateNonConst();
}

moveit::core::RobotStatePtr get_current_state_updated(std::shared_ptr<planning_scene::PlanningScene>& planning_scene,
py::object update)
{
moveit_msgs::msg::RobotState robot_state_cpp;
py::module_ rclpy_serialization = py::module::import("rclpy.serialization");
py::bytes serialized_msg = rclpy_serialization.attr("serialize_message")(update);
deserializeMsg(serialized_msg, robot_state_cpp);

return planning_scene->getCurrentStateUpdated(robot_state_cpp);
}
// moveit::core::RobotStatePtr get_current_state_updated(std::shared_ptr<planning_scene::PlanningScene>& planning_scene,
// py::object update)
//{
// moveit_msgs::msg::RobotState robot_state_cpp;
// py::module_ rclpy_serialization = py::module::import("rclpy.serialization");
// py::bytes serialized_msg = rclpy_serialization.attr("serialize_message")(update);
// deserializeMsg(serialized_msg, robot_state_cpp);
//
// return planning_scene->getCurrentStateUpdated(robot_state_cpp);
// }

void set_current_state(std::shared_ptr<planning_scene::PlanningScene>& planning_scene, py::object& robot_state)
{
Expand Down
16 changes: 13 additions & 3 deletions python/moveit_py/src/moveit_py/moveit_core/planning_scene/planning_scene.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,9 +43,17 @@ namespace moveit_py
{
namespace bind_planning_scene
{
void apply_planning_scene_world(std::shared_ptr<planning_scene::PlanningScene>& planning_scene,
py::object& planning_scene_world_msg);

// void apply_collision_object(std::shared_ptr<planning_scene::PlanningScene>& planning_scene,
// py::object& collision_object);

void apply_collision_object(std::shared_ptr<planning_scene::PlanningScene>& planning_scene,
py::object& collision_object);
py::object& collision_object_msg, std::optional<py::object>& color_msg);

// void apply_collision_object(std::shared_ptr<planning_scene::PlanningScene>& planning_scene, py::object&
// collision_object_msg, py::object& color_msg);

void apply_attached_collision_object(std::shared_ptr<planning_scene::PlanningScene>& planning_scene,
py::object& attached_collision_object);
Expand All @@ -55,6 +63,8 @@ void apply_octomap(std::shared_ptr<planning_scene::PlanningScene>& planning_scen
Eigen::MatrixXd get_frame_transform(std::shared_ptr<planning_scene::PlanningScene>& planning_scene,
const std::string& id);

py::object get_planning_scene_msg(std::shared_ptr<planning_scene::PlanningScene>& planning_scene);

bool is_path_valid(std::shared_ptr<planning_scene::PlanningScene>& planning_scene,
robot_trajectory::RobotTrajectory& robot_trajectory, std::string& group, bool verbose);

Expand All @@ -68,8 +78,8 @@ bool is_state_constrained(std::shared_ptr<planning_scene::PlanningScene>& planni

moveit::core::RobotState& get_current_state(std::shared_ptr<planning_scene::PlanningScene>& planning_scene);

moveit::core::RobotStatePtr get_current_state_updated(std::shared_ptr<planning_scene::PlanningScene>& planning_scene,
py::object update);
// moveit::core::RobotStatePtr get_current_state_updated(std::shared_ptr<planning_scene::PlanningScene>& planning_scene,
// py::object update);

void set_current_state(std::shared_ptr<planning_scene::PlanningScene>& planning_scene, py::object& robot_state);
} // namespace bind_planning_scene
Expand Down
26 changes: 24 additions & 2 deletions python/moveit_py/src/moveit_py/moveit_core/robot_state/robot_state.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,14 +43,36 @@ namespace moveit_py
{
namespace bind_robot_state
{

void update(std::shared_ptr<moveit::core::RobotState>& robot_state, bool force, std::string& category)
{
if (category == "all")
{
robot_state->update(force);
}
else if (category == "links_only")
{
robot_state->updateLinkTransforms();
}
else if (category == "collisions_only")
{
robot_state->updateCollisionBodyTransforms();
}
else
{
throw std::invalid_argument("Invalid category");
}
}

Eigen::MatrixXd get_frame_transform(std::shared_ptr<moveit::core::RobotState>& robot_state, std::string& frame_id)
{
bool* frame_found;
auto transformation = robot_state->getFrameTransform(frame_id, frame_found);
return transformation.matrix();
}

std::vector<Eigen::MatrixXd> get_global_link_transform(std::shared_ptr<moveit::core::RobotState>& robot_state, std::string& link_name)
std::vector<Eigen::MatrixXd> get_global_link_transform(std::shared_ptr<moveit::core::RobotState>& robot_state,
std::string& link_name)
{
auto transformation = robot_state->getGlobalLinkTransform(link_name);
std::vector<Eigen::MatrixXd> transforms;
Expand Down Expand Up @@ -96,7 +118,7 @@ bool set_from_ik(std::shared_ptr<moveit::core::RobotState>& robot_state, const s
const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(joint_model_group_name);

geometry_msgs::msg::Pose pose_cpp = PoseToCpp(geometry_pose);

return robot_state->setFromIK(joint_model_group, pose_cpp, tip_name, timeout);
}

Expand Down
4 changes: 3 additions & 1 deletion python/moveit_py/src/moveit_py/moveit_core/robot_state/robot_state.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,10 +45,12 @@ namespace moveit_py
{
namespace bind_robot_state
{
void update(std::shared_ptr<moveit::core::RobotState>& robot_state, bool force, std::string& category);

Eigen::MatrixXd get_frame_transform(std::shared_ptr<moveit::core::RobotState>& robot_state, std::string& frame_id);

std::vector<Eigen::MatrixXd> get_global_link_transform(std::shared_ptr<moveit::core::RobotState>& robot_state, std::string& link_name);
std::vector<Eigen::MatrixXd> get_global_link_transform(std::shared_ptr<moveit::core::RobotState>& robot_state,
std::string& link_name);

py::object get_pose(std::shared_ptr<moveit::core::RobotState>& robot_state, std::string link_name);

Expand Down
17 changes: 16 additions & 1 deletion python/moveit_py/src/moveit_py/planning.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -191,6 +191,21 @@ PYBIND11_MODULE(planning, m)
Stops the scene monitor.
)mydelimiter")

.def("start_state_monitor", &planning_scene_monitor::PlanningSceneMonitor::startStateMonitor,
R"mydelimiter(
Starts the state monitor.
)mydelimiter")

.def("stop_state_monitor", &planning_scene_monitor::PlanningSceneMonitor::stopStateMonitor,
R"mydelimiter(
Stops the state monitor.
)mydelimiter")

.def("wait_for_current_robot_state", &planning_scene_monitor::PlanningSceneMonitor::waitForCurrentRobotState,
R"mydelimiter(
Waits for the current robot state to be received.
)mydelimiter")

.def("clear_octomap", &planning_scene_monitor::PlanningSceneMonitor::clearOctomap,
R"mydelimiter(
Clears the octomap.
Expand Down Expand Up @@ -370,7 +385,7 @@ PYBIND11_MODULE(planning, m)
R"mydelimiter(
Specify the workspace bounding box.
The box is specified in the planning frame (i.e. relative to the robot root link start position). This is useful when the planning group contains the root joint of the robot – i.e. when planning motion for the robot relative to the world.
The box is specified in the planning frame (i.e. relative to the robot root link start position). The workspace applies only to the root joint of a mobile robot (driving base, quadrotor) and does not limit the workspace of a robot arm.
Args:
min_x (float): The minimum x value of the workspace.
Expand Down

0 comments on commit 88110be

Please sign in to comment.