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

Make simplified trajectory processing work

Browse Source
This commit is contained in:
Nicolas Hiillos
2023-01-26 10:08:46 +02:00
parent e9cc39d155
commit 981723a3a9
1 changed files with 22 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
src/lite6_controller/src/lite6_controller.cpp
View File

@@ -171,11 +171,11 @@ public:
// Set limits for A4 paper // Set limits for A4 paper
float xlim_lower = 0.1; float xlim_lower = 0.1;
float xlim_upper = 0.5; float xlim_upper = 0.3;
float ylim_lower = -0.2; float ylim_lower = -0.2;
float ylim_upper = 0.2; float ylim_upper = 0.2;
float zlim_lower = 0.1; float zlim_lower = 0.1;
float zlim_upper = 0.3; float zlim_upper = 0.2;
/** /**
* Function that translates an input value with a given range to a value within another range. * Function that translates an input value with a given range to a value within another range.
@@ -221,12 +221,13 @@ public:
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 ? "" : "FAILED"); RCLCPP_INFO(this->get_logger(), "Plan (pose goal) %s", success ? "SUCCEEDED" : "FAILED");
// Append segment to complete trajectory // Append segment to complete trajectory
trajectory->joint_trajectory.points.insert(trajectory->joint_trajectory.points.end(), trajectory->joint_trajectory.points.insert(trajectory->joint_trajectory.points.end(),
plan.trajectory_.joint_trajectory.points.begin(), plan.trajectory_.joint_trajectory.points.begin(),
plan.trajectory_.joint_trajectory.points.end()); plan.trajectory_.joint_trajectory.points.end());
trajectory->joint_trajectory.joint_names = plan.trajectory_.joint_trajectory.joint_names;
move_group.clearPoseTarget(); move_group.clearPoseTarget();
} }
@@ -275,19 +276,27 @@ public:
addPoseToTrajectory(p, &trajectory); addPoseToTrajectory(p, &trajectory);
// set move_group start state to final pose of trajectory // set move_group start state to final pose of trajectory
rt.setRobotTrajectoryMsg(*(move_group.getCurrentState()), trajectory); //RCLCPP_INFO(this->get_logger(), "setRobotTrajectoryMsg");
moveit::core::RobotState robot_state = rt.getLastWayPoint(); //rt.setRobotTrajectoryMsg(*(move_group.getCurrentState(100)), trajectory);
const Eigen::Isometry3d& eef_transform = robot_state.getGlobalLinkTransform(move_group.getEndEffectorLink()); //RCLCPP_INFO(this->get_logger(), "getLastWayPoint");
geometry_msgs::msg::Pose pose; //moveit::core::RobotState robot_state = rt.getLastWayPoint();
pose = Eigen::toMsg(eef_transform); //RCLCPP_INFO(this->get_logger(), "eef");
//tf2::toMsg(eef_transform, pose); //const Eigen::Isometry3d& eef_transform = robot_state.getGlobalLinkTransform(move_group.getEndEffectorLink());
move_group.setStartStateToCurrentState(); //geometry_msgs::msg::Pose pose;
//pose = Eigen::toMsg(eef_transform);
////tf2::toMsg(eef_transform, pose);
//move_group.setStartStateToCurrentState();
RCLCPP_INFO(this->get_logger(), "Executing trajectory with %ld points", trajectory.joint_trajectory.points.size());
// TODO remove
this->move_group.execute(trajectory);
trajectory = moveit_msgs::msg::RobotTrajectory();
} }
//double fraction = this->move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); //double fraction = this->move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
//RCLCPP_INFO(this->get_logger(), "Visualizing plan 4 (Cartesian path) (%.2f%% achieved)", fraction * 100.0); //RCLCPP_INFO(this->get_logger(), "Visualizing plan 4 (Cartesian path) (%.2f%% achieved)", fraction * 100.0);
this->move_group.execute(trajectory);
//waypoints.clear(); //waypoints.clear();
@@ -334,7 +343,8 @@ int main(int argc, char ** argv)
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Setting up lite6_controller"); RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Setting up lite6_controller");
lite6->setup(); lite6->setup();
rclcpp::executors::SingleThreadedExecutor executor; //rclcpp::executors::SingleThreadedExecutor executor;
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(lite6); executor.add_node(lite6);
executor.spin(); executor.spin();