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

Fixed move_group_interface

Browse source 
Launchfile changes probably irrelevant but i'm committing every change
just in case
This commit is contained in:
Thomas Muller 2022-03-21 23:02:55 -04:00
parent 9fdd5cf353
commit 91a042ef9f
Signed by: thomas
GPG key ID: AF006EB730564952
3 changed files with 45 additions and 45 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -54,7 +54,9 @@ def generate_launch_description():
get_yaml(join(config_root, 'ompl_planning.yaml')), get_yaml(join(config_root, 'ompl_planning.yaml')),
get_yaml(join(config_root, 'trajectory.yaml')), get_yaml(join(config_root, 'trajectory.yaml')),
get_yaml(join(config_root, 'planning_scene.yaml')), get_yaml(join(config_root, 'planning_scene.yaml')),
get_yaml(join(config_root, "ar3_controllers.yaml"))], get_yaml(join(config_root, "ar3_controllers.yaml")),
{ "default_planning_pipeline": "ompl" },
{ "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }]
), ),
# Our joystick node # Our joystick node
@ -133,7 +135,7 @@ def generate_launch_description():
get_yaml(join(config_root, 'planning_scene.yaml')), get_yaml(join(config_root, 'planning_scene.yaml')),
get_yaml(join(config_root, "ar3_controllers.yaml")), get_yaml(join(config_root, "ar3_controllers.yaml")),
{ "default_planning_pipeline": "move_group" }, { "default_planning_pipeline": "move_group" },
{ "planning_pipelines": [ "ompl", "move_group"] } { "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }
], ],
), ),

2
src/cobot_platform/cobot_platform/move.py
View file

@ -23,7 +23,7 @@ class CobotMove(Node):
self._cur = [0.0] * 6 self._cur = [0.0] * 6
# create a timer to update all the joint angles at once # create a timer to update all the joint angles at once
self._timer = self.create_timer(1 / 40, self._dosomething) self._timer = self.create_timer(1 / 20, self._dosomething)
# Set joint angles independantly # Set joint angles independantly
base_topic = 'move/' base_topic = 'move/'

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

@ -5,8 +5,9 @@
#include <thread> #include <thread>
// MoveitCpp // MoveitCpp
#include <moveit/moveit_cpp/moveit_cpp.h> #include <moveit/common_planning_interface_objects/common_objects.h>
#include <moveit/moveit_cpp/planning_component.h> #include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/rdf_loader/rdf_loader.h>
#include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp"
@ -15,66 +16,66 @@ const std::string PLANNING_GROUP = "ar3_arm";
class ScanNPlan : public rclcpp::Node { class ScanNPlan : public rclcpp::Node {
public: public:
ScanNPlan() : Node("scan_n_plan") { ScanNPlan() : Node("scan_n_plan") {
RCLCPP_INFO(this->get_logger(), "SNP START");
// Create /move_pose subscriber // Create /move_pose subscriber
pose_sub_ = this->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");
relative_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
"move_relative_pose",
rclcpp::QoS(1),
std::bind(&ScanNPlan::relativeMoveCallback, this, std::placeholders::_1)
);
}
void init() {
move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
shared_from_this(), std::string("ar3_arm"));
} }
private: private:
void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) { void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) {
(void) p;
RCLCPP_INFO(this->get_logger(), "GOT MSG"); RCLCPP_INFO(this->get_logger(), "GOT MSG");
// Get a shared pointer to the MoveItCpp library to use our planning scene and getting states
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"); move_group_->setPoseTarget(p->pose);
//psm->startSceneMonitor();
// ?????? moveit::planning_interface::MoveGroupInterface::Plan plan;
moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService(); bool success = (move_group_->plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(this->get_logger(), "Did work? %s", success ? "yes" : "no");
// planning_scene_monitor::LockedPlanningSceneRW scene{moveit_cpp_ptr->getPlanningSceneMonitor()}; move_group_->move();
RCLCPP_INFO(this->get_logger(), "NEW PLANNING"); }
auto planning_components = std::make_shared<moveit_cpp::PlanningComponent>(PLANNING_GROUP, moveit_cpp_ptr); void relativeMoveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) {
RCLCPP_INFO(this->get_logger(), "GET MODEL"); RCLCPP_INFO(this->get_logger(), "GOT MSG");
auto robot_model_ptr = moveit_cpp_ptr->getRobotModel();
RCLCPP_INFO(this->get_logger(), "GET START STATE");
auto robot_start_state = planning_components->getStartState();
RCLCPP_INFO(this->get_logger(), "SETSTART TO CUR"); moveit::core::RobotStatePtr robot_state = move_group_->getCurrentState();
planning_components->setStartStateToCurrentState(); const Eigen::Isometry3d &end_effector_state = robot_state->getGlobalLinkTransform(move_group_->getEndEffectorLink());
RCLCPP_INFO_STREAM(this->get_logger(), "POS: \n" << end_effector_state.translation() << "\n");
RCLCPP_INFO_STREAM(this->get_logger(), "ROT: \n" << end_effector_state.rotation() << "\n");
// Set the goal to the request pose geometry_msgs::msg::Pose target_pose;
RCLCPP_INFO(this->get_logger(), "SET GOAL"); target_pose.position.x = 0.24;
planning_components->setGoal(*p, "base_link"); target_pose.position.y = -0.32;
target_pose.position.z = 0.56;
move_group_->setPoseTarget(target_pose);
// Actually plan moveit::planning_interface::MoveGroupInterface::Plan plan;
RCLCPP_INFO(this->get_logger(), "DO PLAN"); bool success = (move_group_->plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
auto plan_solution = planning_components->plan();
// Plan is valid RCLCPP_INFO(this->get_logger(), "Did work? %s", success ? "yes" : "no");
if (plan_solution) {
// Follow through with the planned path move_group_->move();
planning_components->execute();
}
} }
private: private:
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_; rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr relative_pose_sub_;
//please moveit::planning_interface::MoveGroupInterfacePtr move_group_;
//rclcpp::Node::SharedPtr nh_; moveit::core::RobotModelConstPtr robot_model_;
// moveit_cpp::MoveItCpp::SharedPtr moveit_cpp_ptr_;
// rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
// std::thread thread_executor_spin_;
}; };
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
@ -83,12 +84,9 @@ int main(int argc, char* argv[]) {
auto node = std::make_shared<ScanNPlan>(); auto node = std::make_shared<ScanNPlan>();
RCLCPP_INFO(node->get_logger(), "MoveGroup node started."); RCLCPP_INFO(node->get_logger(), "MoveGroup node started.");
node->init();
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;