robossembler/runtime
7
0
Fork 0
Code Issues 5 Pull requests Projects Releases Packages Wiki Activity Actions

Add minimal pick-and-place case with world embedded

Browse source
This commit is contained in:
Igor Brylyov 2023-03-31 20:24:56 +00:00
parent 209e99a4b3
commit a87fb8a7ec
64 changed files with 2419 additions and 275 deletions
Download patch file Download diff file Expand all files Collapse all files

44
rbs_skill_servers/src/move_cartesian_path_action_server.cpp
View file

@ -86,9 +86,7 @@ private:
goal->target_pose.orientation.x, goal->target_pose.orientation.y, goal->target_pose.orientation.z,
goal->target_pose.orientation.w);
(void)uuid;
if (false) {
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
@ -103,6 +101,10 @@ private:
{
using namespace std::placeholders;
std::thread(std::bind(&MoveCartesianActionServer::execute, this, _1), goal_handle).detach();
// std::thread(
// [this, goal_handle]() {
// execute(goal_handle);
// }).detach();
}
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle)
@ -112,22 +114,15 @@ private:
auto result = std::make_shared<MoveitSendPose::Result>();
moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);
move_group_interface.startStateMonitor();
moveit::core::RobotState start_state(*move_group_interface.getCurrentState());
auto start_pose = move_group_interface.getCurrentPose();
std::vector<geometry_msgs::msg::Pose> waypoints;
RCLCPP_INFO(this->get_logger(), "Current with Pose [%f, %f, %f, %f, %f, %f, %f]",
start_pose.pose.position.x, start_pose.pose.position.y, start_pose.pose.position.z,
start_pose.pose.orientation.x, start_pose.pose.orientation.y, start_pose.pose.orientation.z,
start_pose.pose.orientation.w);
//waypoints.push_back(start_pose.pose);
geometry_msgs::msg::Pose target_pose = start_pose.pose;
//goal->target_pose
target_pose.position.z = target_pose.position.z + goal->target_pose.position.z;
target_pose.position.y = target_pose.position.y + goal->target_pose.position.y;
target_pose.position.x = target_pose.position.x + goal->target_pose.position.x;
auto current_pose = move_group_interface.getCurrentPose();
//waypoints.push_back(current_pose.pose);
geometry_msgs::msg::Pose target_pose = goal->target_pose;
//target_pose.position = goal->target_pose.position;
waypoints.push_back(target_pose);
RCLCPP_INFO(this->get_logger(), "New cartesian target pose [%f, %f, %f]",
@ -135,16 +130,25 @@ private:
//waypoints.push_back(start_pose.pose);
moveit_msgs::msg::RobotTrajectory trajectory;
const double jump_threshold = 0.0;
const double eef_step = 0.01;
const double eef_step = 0.001;
double fraction = move_group_interface.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
waypoints.clear();
if(fraction>0)
{
RCLCPP_INFO(this->get_logger(), "Planning success");
move_group_interface.execute(trajectory);
moveit::core::MoveItErrorCode execute_err_code = move_group_interface.execute(trajectory);
if (execute_err_code == moveit::core::MoveItErrorCode::SUCCESS)
{
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
}else if (execute_err_code == moveit::core::MoveItErrorCode::FAILURE)
{
RCLCPP_ERROR(this->get_logger(), "Failure in move:");
}
//move_group_interface.move();
}else{
RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
goal_handle->abort(result);
}
if (goal_handle->is_canceling()) {
@ -152,10 +156,6 @@ private:
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
return;
}
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
}
}; // class MoveCartesianActionServer