From ad3654a274e8941f40b259a20faf18139fc778ad Mon Sep 17 00:00:00 2001 From: peterdavidfagan Date: Wed, 4 Jan 2023 16:15:39 +0000 Subject: [PATCH] fix header frame_idwq --- .../moveit_ros/moveit_cpp/planning_component.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/moveit_py/src/moveit_py/moveit_ros/moveit_cpp/planning_component.cpp b/moveit_py/src/moveit_py/moveit_ros/moveit_cpp/planning_component.cpp index 60c9bffc0cb..fec4fa0aed6 100644 --- a/moveit_py/src/moveit_py/moveit_ros/moveit_cpp/planning_component.cpp +++ b/moveit_py/src/moveit_py/moveit_ros/moveit_cpp/planning_component.cpp @@ -159,7 +159,7 @@ bool set_goal(std::shared_ptr& planning_component double* ptr = (double*)buf.ptr; geometry_msgs::msg::PoseStamped pose_goal_cpp; - pose_goal_cpp.header.frame_id = link_name; + pose_goal_cpp.header.frame_id = pose_dict["source_frame"].cast(); pose_goal_cpp.pose.position.x = ptr[0]; pose_goal_cpp.pose.position.y = ptr[1]; pose_goal_cpp.pose.position.z = ptr[2]; @@ -175,6 +175,7 @@ bool set_goal(std::shared_ptr& planning_component return planning_component->setGoal(pose_goal_cpp, link_name); } } + // TODO catch specific errors catch (...) { throw std::invalid_argument("Invalid pose_goal dictionary"); @@ -185,10 +186,16 @@ bool set_goal(std::shared_ptr& planning_component { // iterate through list of constraints and convert to C++ std::vector constraints_vec_cpp; - py::list constraints = motion_plan_constraints.value().cast(); + py::list constraints = motion_plan_constraints.value(); + py::module_ rclpy_serialization = py::module::import("rclpy.serialization"); for (int i = 0; i < py::len(constraints); i++) { - moveit_msgs::msg::Constraints constraints_cpp = ConstraintsToCpp(constraints[i]); + // convert python object to cpp constraints message + moveit_msgs::msg::Constraints constraints_cpp; + py::bytes serialized_msg = rclpy_serialization.attr("serialize_message")(constraints[i]); + deserializeMsg(serialized_msg, constraints_cpp); + + // moveit_msgs::msg::Constraints constraints_cpp = ConstraintsToCpp(constraints[i]); constraints_vec_cpp.push_back(constraints_cpp); }