nic/drawing-robot-ros2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Remove excessive calls to getCurrentState()

Browse Source
This commit is contained in:
Nicolas Hiillos
2023-02-15 16:22:57 +02:00
parent 4b61a591aa
commit 05909444d6
1 changed files with 16 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
src/lite6_controller/src/lite6_controller.cpp
View File

@@ -214,7 +214,7 @@ public:
/** /**
* Creates a trajectory for a pose and appends it to a given trajectory * Creates a trajectory for a pose and appends it to a given trajectory
*/ */
bool addPoseToTrajectory(geometry_msgs::msg::PoseStamped pose, moveit_msgs::msg::RobotTrajectory *trajectory) bool addPoseToTrajectory(geometry_msgs::msg::PoseStamped pose, moveit_msgs::msg::RobotTrajectory *trajectory, moveit::core::RobotStatePtr state)
{ {
pose = translatePose(pose); pose = translatePose(pose);
move_group.setPoseTarget(pose); move_group.setPoseTarget(pose);
@@ -224,21 +224,21 @@ public:
//move_group.setPlanningPipelineId("PTP"); //move_group.setPlanningPipelineId("PTP");
move_group.setPlannerId("PTP"); move_group.setPlannerId("PTP");
robot_trajectory::RobotTrajectory previous_trajectory(move_group.getCurrentState()->getRobotModel(), move_group.getName()); robot_trajectory::RobotTrajectory previous_trajectory(state->getRobotModel(), move_group.getName());
previous_trajectory.setRobotTrajectoryMsg(*move_group.getCurrentState(), *trajectory); previous_trajectory.setRobotTrajectoryMsg(*state, *trajectory);
moveit::planning_interface::MoveGroupInterface::Plan plan; moveit::planning_interface::MoveGroupInterface::Plan plan;
bool success = (move_group.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS); bool success = (move_group.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(this->get_logger(), "Plan (pose goal) %s", success ? "SUCCEEDED" : "FAILED"); RCLCPP_INFO(this->get_logger(), "Plan (pose goal) %s", success ? "SUCCEEDED" : "FAILED, trying approximate method (if enabled)");
if (success) if (success)
{ {
robot_trajectory::RobotTrajectory next_trajectory(move_group.getCurrentState()->getRobotModel(), move_group.getName()); robot_trajectory::RobotTrajectory next_trajectory(state->getRobotModel(), move_group.getName());
next_trajectory.setRobotTrajectoryMsg(*move_group.getCurrentState(), plan.trajectory_); next_trajectory.setRobotTrajectoryMsg(*state, plan.trajectory_);
// append trajectory, with time step of 2.0, not skipping any points // append trajectory, with time step of 2.0, not skipping any points
previous_trajectory.append(next_trajectory, 0.01, 0); previous_trajectory.append(next_trajectory, 0.000001, 0);
*trajectory = moveit_msgs::msg::RobotTrajectory(); *trajectory = moveit_msgs::msg::RobotTrajectory();
previous_trajectory.getRobotTrajectoryMsg(*trajectory); previous_trajectory.getRobotTrajectoryMsg(*trajectory);
@@ -251,7 +251,9 @@ public:
} }
else else
{ {
success = addPoseToTrajectoryApproximate(pose, trajectory); // Unsafe approximate solution
// Likely to break pens
//success = addPoseToTrajectoryApproximate(pose, trajectory);
} }
move_group.clearPoseTarget(); move_group.clearPoseTarget();
return success; return success;
@@ -280,7 +282,7 @@ public:
next_trajectory.setRobotTrajectoryMsg(*move_group.getCurrentState(), plan.trajectory_); next_trajectory.setRobotTrajectoryMsg(*move_group.getCurrentState(), plan.trajectory_);
// append trajectory, with time step of 2.0, not skipping any points // append trajectory, with time step of 2.0, not skipping any points
previous_trajectory.append(next_trajectory, 0.01, 0); previous_trajectory.append(next_trajectory, 0.0001, 0);
*trajectory = moveit_msgs::msg::RobotTrajectory(); *trajectory = moveit_msgs::msg::RobotTrajectory();
previous_trajectory.getRobotTrajectoryMsg(*trajectory); previous_trajectory.getRobotTrajectoryMsg(*trajectory);
@@ -332,18 +334,21 @@ public:
robot_trajectory::RobotTrajectory single_trajectory(move_group.getCurrentState()->getRobotModel(), move_group.getName()); robot_trajectory::RobotTrajectory single_trajectory(move_group.getCurrentState()->getRobotModel(), move_group.getName());
move_group.setStartStateToCurrentState(); move_group.setStartStateToCurrentState();
moveit::core::RobotStatePtr move_group_state = move_group.getCurrentState();
for (auto p : goal->motion.path) for (auto p : goal->motion.path)
{ {
//RCLCPP_INFO(this->get_logger(), "Planning trajectory"); //RCLCPP_INFO(this->get_logger(), "Planning trajectory");
// Append next pose to trajectory // Append next pose to trajectory
if (!addPoseToTrajectory(p, &multi_trajectory)) continue; if (!addPoseToTrajectory(p, &multi_trajectory, move_group_state)) continue;
// set move_group start state to final pose of trajectory // set move_group start state to final pose of trajectory
//RCLCPP_INFO(this->get_logger(), "setRobotTrajectoryMsg"); //RCLCPP_INFO(this->get_logger(), "setRobotTrajectoryMsg");
single_trajectory.setRobotTrajectoryMsg(*move_group.getCurrentState(), multi_trajectory); single_trajectory.setRobotTrajectoryMsg(*move_group_state, multi_trajectory);
//rt.setRobotTrajectoryMsg(rt.getLastWayPoint(), trajectory); //rt.setRobotTrajectoryMsg(rt.getLastWayPoint(), trajectory);
//RCLCPP_INFO(this->get_logger(), "getLastWayPoint"); //RCLCPP_INFO(this->get_logger(), "getLastWayPoint");
//
//moveit::core::RobotState robot_state = single_trajectory.getLastWayPoint();
moveit::core::RobotState robot_state = single_trajectory.getLastWayPoint(); moveit::core::RobotState robot_state = single_trajectory.getLastWayPoint();
//RCLCPP_INFO(this->get_logger(), "eef"); //RCLCPP_INFO(this->get_logger(), "eef");
//const Eigen::Isometry3d& eef_transform = robot_state.getGlobalLinkTransform(move_group.getEndEffectorLink()); //const Eigen::Isometry3d& eef_transform = robot_state.getGlobalLinkTransform(move_group.getEndEffectorLink());