33 lines
976 B
C++
33 lines
976 B
C++
//
|
|
// Created by bastian on 28.03.22.
|
|
//
|
|
|
|
#ifndef BUILD_MOVECONNECTION_H
|
|
#define BUILD_MOVECONNECTION_H
|
|
|
|
#include <rclcpp/rclcpp.hpp>
|
|
#include <moveit/move_group_interface/move_group_interface.h>
|
|
#include <moveit/planning_scene_interface/planning_scene_interface.h>
|
|
#include <rclcpp_action/client.hpp>
|
|
|
|
class MoveConnection {
|
|
private:
|
|
enum {IDLE,RUNNING,SPEED_CHANGE} state;
|
|
moveit::planning_interface::MoveGroupInterface moveGroup;
|
|
std::shared_ptr<geometry_msgs::msg::Pose> lastTarget = nullptr;
|
|
std::function<void(bool)> lastCallback = [](__attribute__((unused))bool finished) {};
|
|
double currentSpeed = 1;
|
|
std::mutex lock;
|
|
public:
|
|
MoveConnection(const std::shared_ptr<rclcpp::Node>& node, const std::string& planningGroup);
|
|
|
|
void planAndExecute(const std::shared_ptr<geometry_msgs::msg::Pose> &pose,const std::function<void(bool)> &callback);
|
|
|
|
void stop();
|
|
|
|
void setSpeedMultiplier(double speed);
|
|
};
|
|
|
|
|
|
#endif //BUILD_MOVECONNECTION_H
|