thomas/Cobots
2
1
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

Unbroke most of mgi node

Browse source
This commit is contained in:
Thomas Muller 2022-03-02 00:31:55 +00:00
parent 75e238ec5f
commit f20e8cce95
2 changed files with 28 additions and 20 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

5
src/ar3_config/launch/dev.launch.py
View file

@ -46,6 +46,11 @@ def generate_launch_description():
package='move_group_interface', package='move_group_interface',
executable='move_group_interface_node', executable='move_group_interface_node',
output='screen', output='screen',
parameters=[robot_description, robot_description_semantic, robot_description_kinematics,
get_yaml(join(config_root, 'ompl_planning.yaml')),
get_yaml(join(config_root, 'trajectory.yaml')),
get_yaml(join(config_root, 'planning_scene.yaml')),
get_yaml(join(config_root, "ar3_controllers.yaml"))],
), ),
Node( Node(

43
src/move_group_interface/src/move_group_interface_node.cpp
View file

@ -16,34 +16,26 @@ class ScanNPlan : public rclcpp::Node {
public: public:
ScanNPlan() : Node("scan_n_plan") { ScanNPlan() : Node("scan_n_plan") {
// create the node and start it // create the node and start it
nh_ = std::make_shared<rclcpp::Node>("scan_n_plan"); // nh_ = std::make_shared<Node>(reinterpret_cast<Node *>(this));
//nh_ = shared_from_this();
// spin the node RCLCPP_INFO(this->get_logger(), "SNP START");
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
executor_->add_node(nh_);
/*
RCLCPP_INFO(nh_->get_logger(), "scan and plan in constructor");
auto spin = [this]() {
while(rclcpp::ok()) {
executor_->spin_once();
RCLCPP_INFO(nh_->get_logger(), "scan and plan still running");
}
};
thread_executor_spin_ = std::thread(spin);
*/
// create subscriber // create subscriber
pose_sub_ = nh_->create_subscription<geometry_msgs::msg::PoseStamped>( pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
"move_pose", "move_pose",
rclcpp::QoS(1), rclcpp::QoS(1),
std::bind(&ScanNPlan::moveCallback, this, std::placeholders::_1) std::bind(&ScanNPlan::moveCallback, this, std::placeholders::_1)
); );
RCLCPP_INFO(this->get_logger(), "SUB EXIST");
} }
private: private:
void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) { void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) {
auto moveit_cpp_ptr = std::make_shared<moveit_cpp::MoveItCpp>(nh_); (void) p;
RCLCPP_INFO(this->get_logger(), "GOT MSG");
auto moveit_cpp_ptr = std::make_shared<moveit_cpp::MoveItCpp>(shared_from_this());
RCLCPP_INFO(this->get_logger(), "GOT MOVEIT");
//auto psm = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description"); //auto psm = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
//psm->startSceneMonitor(); //psm->startSceneMonitor();
@ -51,17 +43,23 @@ private:
// planning_scene_monitor::LockedPlanningSceneRW scene{moveit_cpp_ptr->getPlanningSceneMonitor()}; // planning_scene_monitor::LockedPlanningSceneRW scene{moveit_cpp_ptr->getPlanningSceneMonitor()};
RCLCPP_INFO(this->get_logger(), "NEW PLANNING");
auto planning_components = std::make_shared<moveit_cpp::PlanningComponent>(PLANNING_GROUP, moveit_cpp_ptr); auto planning_components = std::make_shared<moveit_cpp::PlanningComponent>(PLANNING_GROUP, moveit_cpp_ptr);
RCLCPP_INFO(this->get_logger(), "GET MODEL");
auto robot_model_ptr = moveit_cpp_ptr->getRobotModel(); auto robot_model_ptr = moveit_cpp_ptr->getRobotModel();
RCLCPP_INFO(this->get_logger(), "GET START STATE");
auto robot_start_state = planning_components->getStartState(); auto robot_start_state = planning_components->getStartState();
RCLCPP_INFO(this->get_logger(), "SETSTART TO CUR");
planning_components->setStartStateToCurrentState(); planning_components->setStartStateToCurrentState();
// set the goal to the request pose // set the goal to the request pose
RCLCPP_INFO(this->get_logger(), "SET GOAL");
planning_components->setGoal(*p, "base_link"); planning_components->setGoal(*p, "base_link");
// actually plan // actually plan
RCLCPP_INFO(this->get_logger(), "DO PLAN");
auto plan_solution = planning_components->plan(); auto plan_solution = planning_components->plan();
// plan is valid // plan is valid
@ -73,9 +71,10 @@ private:
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_; rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
//please //please
rclcpp::Node::SharedPtr nh_; //rclcpp::Node::SharedPtr nh_;
rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; // moveit_cpp::MoveItCpp::SharedPtr moveit_cpp_ptr_;
std::thread thread_executor_spin_; // rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
// std::thread thread_executor_spin_;
}; };
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
@ -86,6 +85,10 @@ int main(int argc, char* argv[]) {
rclcpp::spin(node); rclcpp::spin(node);
// spin the node
// executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
// executor_->add_node(nh_);
RCLCPP_INFO(node->get_logger(), "Terminating MoveGroup"); RCLCPP_INFO(node->get_logger(), "Terminating MoveGroup");
rclcpp::shutdown(); rclcpp::shutdown();
return 0; return 0;