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

Add magic coordinates and debug functions

Browse Source
This commit is contained in:
Nicolas Hiillos
2023-03-02 17:51:21 +02:00
parent aece2ac7b6
commit 8e533e08bd
1 changed files with 195 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files

197
src/lite6_controller/src/lite6_controller.cpp
View File

@@ -4,6 +4,9 @@
#include <chrono>
//#include <queue>
#include <fstream>
#include <iostream>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <moveit_msgs/msg/collision_object.hpp>
@@ -75,18 +78,12 @@ public:
// Set limits for A4 paper
// 297x210
//float xlim_lower = 0.30;
//float xlim_upper = 0.305;
//float ylim_lower = -0.02;
//float ylim_upper = 0.00;
//float zlim_lower = 0.25;
//float zlim_upper = 0.255;
float xlim_lower = 0.20;
float xlim_upper = 0.25;
float ylim_lower = -0.05;
float ylim_upper = 0.05;
float zlim_lower = 0.157;
float zlim_upper = 0.17;
float xlim_lower = 0.10;
float xlim_upper = 0.30;
float ylim_lower = -0.14;
float ylim_upper = 0.14;
float zlim_lower = 0.19;
float zlim_upper = 0.21;
//bool moved = false;
//
@@ -191,36 +188,16 @@ public:
}
/**
* Callback that executes path on robot
*
*/
virtual void executePath(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteMotion>> goal_handle)
moveit_msgs::msg::MotionSequenceRequest createMotionSequenceRequest(const std::vector<geometry_msgs::msg::PoseStamped> *path)
{
RCLCPP_INFO(this->get_logger(), "Executing goal");
rclcpp::Rate loop_rate(20);
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<robot_interfaces::action::ExecuteMotion::Feedback>();
auto result = std::make_shared<robot_interfaces::action::ExecuteMotion::Result>();
// getting current state of robot from environment
//if (!moveit_cpp_->getPlanningSceneMonitor()->requestPlanningSceneState())
//{
// RCLCPP_ERROR(this->get_logger(), "Failed to get planning scene");
// return;
//}
//moveit::core::RobotStatePtr start_robot_state = moveit_cpp_->getCurrentState(2.0);
moveit_msgs::msg::MotionSequenceRequest msr = moveit_msgs::msg::MotionSequenceRequest();
//waypoints.push_back(move_group.getCurrentPose().pose);
std::string ee_link = move_group.getLinkNames().back();
RCLCPP_INFO(this->get_logger(), "Got ee_link: %s", ee_link.c_str());
long n = 0;
for (auto p : goal->motion.path)
for (auto p : *path)
{
// skip first
//if (n <= 0) continue;
//n--;
//RCLCPP_INFO(this->get_logger(), "Creating MSI");
moveit_msgs::msg::MotionSequenceItem msi = moveit_msgs::msg::MotionSequenceItem();
@@ -260,7 +237,8 @@ public:
mpr.goal_constraints.push_back(pose_goal);
msi.req = mpr;
msi.blend_radius = 0.000000001; //TODO make configurable
msi.blend_radius = 0.0; //TODO make configurable
//msi.blend_radius = 0.000000001; //TODO make configurable
msr.items.push_back(msi);
}
msr.items.back().blend_radius = 0.0; // Last element blend must be 0
@@ -268,47 +246,162 @@ public:
moveit_msgs::msg::RobotState state_msg;
moveit::core::robotStateToRobotStateMsg(*move_group_state, state_msg);
msr.items.front().req.start_state = state_msg;
return msr;
}
/**
*
*/
moveit_msgs::msg::MotionSequenceRequest createMotionSequenceRequest(const std::vector<geometry_msgs::msg::PoseStamped> *path, double x_offset, double y_offset, double z_offset)
{
this->xlim_lower += x_offset;
this->xlim_upper += x_offset;
this->ylim_lower += y_offset;
this->ylim_upper += y_offset;
this->zlim_lower += z_offset;
this->zlim_upper += z_offset;
moveit_msgs::msg::MotionSequenceRequest msr = createMotionSequenceRequest(path);
this->xlim_lower -= x_offset;
this->xlim_upper -= x_offset;
this->ylim_lower -= y_offset;
this->ylim_upper -= y_offset;
this->zlim_lower -= z_offset;
this->zlim_upper -= z_offset;
return msr;
}
/**
*
*/
static void appendLineToFile(std::string filepath, std::string line)
{
std::ofstream file;
file.open(filepath, std::ios::out | std::ios::app);
if (file.fail())
throw std::ios_base::failure(std::strerror(errno));
file.exceptions(file.exceptions() | std::ios::failbit | std::ifstream::badbit);
file << line << std::endl;
}
/**
*
*/
std::string pointsToString(const std::vector<geometry_msgs::msg::PoseStamped> *path, double x_offset, double y_offset, double z_offset)
{
std::string out = "";
this->xlim_lower += x_offset;
this->xlim_upper += x_offset;
this->ylim_lower += y_offset;
this->ylim_upper += y_offset;
this->zlim_lower += z_offset;
this->zlim_upper += z_offset;
for (auto p : *path)
{
auto position = translatePose(p).pose.position;
out = out + std::to_string(position.x) + " " +
std::to_string(position.y) + " " +
std::to_string(position.z) + ",";
}
out = out.substr(0, out.size()-1); //remove trailing comma
this->xlim_lower -= x_offset;
this->xlim_upper -= x_offset;
this->ylim_lower -= y_offset;
this->ylim_upper -= y_offset;
this->zlim_lower -= z_offset;
this->zlim_upper -= z_offset;
return out;
}
/**
* Tests path with offsets in multiple locations and logs them to OUTPUT.csv
*/
void planAndLogOffset(const std::vector<geometry_msgs::msg::PoseStamped> *path)
{
double res = 0.05;
double bot = -0.5;
double top = 0.5;
for (double x = bot; x <= top; x+=res)
{
for (double y = bot; y <= top; y+=res)
{
for (double z = bot; z <= top; z+=res)
{
auto msr = createMotionSequenceRequest(path, x, y, z);
auto req = std::make_shared<moveit_msgs::srv::GetMotionSequence::Request>();
req->request = msr;
auto res = sequence_client_->async_send_request(req);
auto ts = res.get()->response.planned_trajectories;
//RCLCPP_INFO(this->get_logger(), "Got %ld trajectories", ts.size());
std::string status = "";
if (ts.size() > 0)
{
status = "success";
}
else
{
status = "failure";
}
status = status + "," + pointsToString(path,x,y,z);
appendLineToFile("OUTPUT.csv", status);
}
}
}
}
/**
* Callback that executes path on robot
*/
virtual void executePath(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteMotion>> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing goal");
rclcpp::Rate loop_rate(20);
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<robot_interfaces::action::ExecuteMotion::Feedback>();
auto result = std::make_shared<robot_interfaces::action::ExecuteMotion::Result>();
auto msr = createMotionSequenceRequest(&goal->motion.path);
RCLCPP_INFO(this->get_logger(), "Created MSR");
//moveit::core::RobotStatePtr move_group_state = move_group.getCurrentState();
//robot_trajectory::RobotTrajectory trajectory(move_group_state.move_group.getName());
//moveit_msgs::msg::RobotTrajectory *trajectory_msg;
//sendMSR(msr);
//planAndLogOffset(&goal->motion.path);
RCLCPP_INFO(this->get_logger(), "Creating req");
auto req = std::make_shared<moveit_msgs::srv::GetMotionSequence::Request>();
RCLCPP_INFO(this->get_logger(), "Setting msr request");
req->request = msr;
RCLCPP_INFO(this->get_logger(), "Sending request to sequence service");
auto res = sequence_client_->async_send_request(req);
// Wait for the result.
RCLCPP_INFO(this->get_logger(), "Waiting for result");
auto status = res.wait_for(30s);
if (status == std::future_status::ready)
{
auto ts = res.get()->response.planned_trajectories;
RCLCPP_INFO(this->get_logger(), "Got %ld trajectories", ts.size());
std::string status = "";
if (ts.size() > 0)
{
status = "success";
RCLCPP_INFO(this->get_logger(), "Got %ld trajectories", ts.size());
RCLCPP_INFO(this->get_logger(), "Executing result");
move_group.execute(ts[0]);
// Check if goal is done
if (rclcpp::ok()) {
status = status + "," + pointsToString(&goal->motion.path,0,0,0);
//appendLineToFile("OUTPUT.csv", status);
result->result = "success";
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Goal succeeded");
return;
}
}
}
else
{
status = "failure";
status = status + "," + pointsToString(&goal->motion.path,0,0,0);
//appendLineToFile("OUTPUT.csv", status);
RCLCPP_ERROR(this->get_logger(), "Planner failed to return trajectory in time");
}
result->result = "failure";
goal_handle->succeed(result);
// abort prevents action from being called
//goal_handle->abort(result);
RCLCPP_ERROR(this->get_logger(), "Goal failed");
}
};