yenon/ROS_Workspace
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed MoveConnection

Browse Source
This commit is contained in:
Bastian Hofmann 2022-03-29 13:19:40 +02:00
parent 406cba6989
commit 58d1d08d01
2 changed files with 48 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
src/btree_nodes/src/nodes/MoveConnection.cpp
View File

@ -3,3 +3,47 @@
// //
#include "MoveConnection.h" #include "MoveConnection.h"
void MoveConnection::planAndExecute(const std::shared_ptr<geometry_msgs::msg::Pose> &pose,
const std::function<void(bool)> &callback) {
lock.lock();
std::cout << "planning" << std::endl;
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (moveGroup.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
if(!success){
callback(false);
return;
}
std::thread([this,callback,pose](){
try {
printf("X:%f Y:%f Z:%f\n",pose->position.x,pose->position.y,pose->position.z);
printf("W:%f X:%f Y:%f Z:%f\n",pose->orientation.w,pose->orientation.x,pose->orientation.y,pose->orientation.z);
moveGroup.setStartStateToCurrentState();
moveGroup.setPoseTarget(*pose);
std::cout << "executing" << std::endl;
bool success = this->moveGroup.move() == moveit::planning_interface::MoveItErrorCode::SUCCESS;
std::cout << "callback" << std::endl;
callback(success);
std::cout << "end" << std::endl;
lock.unlock();
} catch (...) {
std::exception_ptr p = std::current_exception();
std::clog <<(p ? p.__cxa_exception_type()->name() : "null") << std::endl;
}
}).detach();
}
MoveConnection::MoveConnection(const std::shared_ptr<rclcpp::Node> &node, const std::string &planningGroup) : moveGroup(node,planningGroup) {
}
void MoveConnection::stop() {
std::cout << "stopping move" << std::endl;
moveGroup.stop();
std::cout << "move stopped." << std::endl;
}

40
src/btree_nodes/src/nodes/MoveConnection.h
View File

@ -8,50 +8,18 @@
#include <rclcpp/rclcpp.hpp> #include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h> #include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h> #include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <rclcpp_action/client.hpp>
class MoveConnection { class MoveConnection {
private: private:
moveit::planning_interface::MoveGroupInterface moveGroup; moveit::planning_interface::MoveGroupInterface moveGroup;
std::thread running;
std::mutex lock; std::mutex lock;
public: public:
MoveConnection(const std::shared_ptr<rclcpp::Node>& node, const std::string& planningGroup): moveGroup(node,planningGroup) { 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 planAndExecute(const std::shared_ptr<geometry_msgs::msg::Pose> &pose,const std::function<void(bool)> &callback);
lock.lock();
printf("X:%f Y:%f Z:%f\n",pose->position.x,pose->position.y,pose->position.z);
printf("W:%f X:%f Y:%f Z:%f\n",pose->orientation.w,pose->orientation.x,pose->orientation.y,pose->orientation.z);
std::cout << "planning" << std::endl;
moveGroup.setStartStateToCurrentState(); void stop();
moveGroup.setPoseTarget(*pose);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (moveGroup.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
if(!success){
callback(false);
}
running = std::thread([this,callback,my_plan](){
std::cout << "executing" << std::endl;
moveGroup.stop();
bool success = moveGroup.execute(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
std::cout << "callback" << std::endl;
callback(success);
std::cout << "end" << std::endl;
lock.unlock();
});
}
void stop(){
std::cout << "stopping move" << std::endl;
moveGroup.stop();
if(running.joinable()){
running.join();
}
std::cout << "move stopped." << std::endl;
}
}; };