65 lines
1.9 KiB
C++
65 lines
1.9 KiB
C++
//
|
|
// Created by bastian on 28.03.22.
|
|
//
|
|
|
|
#include "MoveConnection.h"
|
|
|
|
MoveConnection::MoveConnection(const std::shared_ptr<rclcpp::Node> &node, const std::string &planningGroup) : moveGroup(node,planningGroup) {}
|
|
|
|
void MoveConnection::planAndExecute(const std::shared_ptr<geometry_msgs::msg::Pose> &pose,
|
|
const std::function<void(bool)> &callback) {
|
|
std::cout<<"planAndExecute."<<std::endl;
|
|
lock.lock();
|
|
state = RUNNING;
|
|
std::cout<<"got lock."<<std::endl;
|
|
lastTarget = pose;
|
|
lastCallback = callback;
|
|
|
|
moveGroup.setMaxVelocityScalingFactor(currentSpeed);
|
|
moveGroup.setMaxAccelerationScalingFactor(1);
|
|
|
|
std::cout<<"Starting planning"<<std::endl;
|
|
|
|
std::thread([&](){
|
|
moveGroup.setPoseTarget(*lastTarget);
|
|
moveGroup.setGoalJointTolerance(0.01);
|
|
moveGroup.setGoalOrientationTolerance(0.01);
|
|
|
|
std::cout<<"Parameters set."<<std::endl;
|
|
|
|
auto finished = moveGroup.move() == moveit::core::MoveItErrorCode::SUCCESS;
|
|
|
|
if(state!=SPEED_CHANGE){
|
|
lastCallback( finished );
|
|
lastTarget = nullptr;
|
|
}
|
|
|
|
state = IDLE;
|
|
lock.unlock();
|
|
}).detach();
|
|
}
|
|
|
|
void MoveConnection::setSpeedMultiplier(const double speed){
|
|
if(currentSpeed!=speed) {
|
|
std::cout<<"Changing speed."<<std::endl;
|
|
currentSpeed = speed;
|
|
auto target = lastTarget;
|
|
auto callback = lastCallback;
|
|
if (target != nullptr) {
|
|
state = SPEED_CHANGE;
|
|
stop();
|
|
std::cout<<"Restarting"<<std::endl;
|
|
planAndExecute(target, callback);
|
|
}
|
|
}
|
|
}
|
|
|
|
void MoveConnection::stop() {
|
|
std::cout<<"Stopping MoveConnection"<<std::endl;
|
|
while (rclcpp::ok() && state!=IDLE){
|
|
moveGroup.stop();
|
|
usleep(1000);
|
|
}
|
|
std::cout<<"Stopped MoveConnection"<<std::endl;
|
|
}
|