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, 'trajectory.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
@ -133,7 +135,7 @@ def generate_launch_description():
get_yaml(join(config_root, 'planning_scene.yaml')),
get_yaml(join(config_root, "ar3_controllers.yaml")),
{ "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
# 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
base_topic = 'move/'

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

@ -5,8 +5,9 @@
#include <thread>
// MoveitCpp
#include <moveit/moveit_cpp/moveit_cpp.h>
#include <moveit/moveit_cpp/planning_component.h>
#include <moveit/common_planning_interface_objects/common_objects.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/rdf_loader/rdf_loader.h>
#include "geometry_msgs/msg/pose_stamped.hpp"
@ -15,66 +16,66 @@ const std::string PLANNING_GROUP = "ar3_arm";
class ScanNPlan : public rclcpp::Node {
public:
ScanNPlan() : Node("scan_n_plan") {
RCLCPP_INFO(this->get_logger(), "SNP START");
// Create /move_pose subscriber
pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
"move_pose",
rclcpp::QoS(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:
void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) {
(void) p;
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");
//psm->startSceneMonitor();
move_group_->setPoseTarget(p->pose);
// ??????
moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService();
moveit::planning_interface::MoveGroupInterface::Plan plan;
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()};
RCLCPP_INFO(this->get_logger(), "NEW PLANNING");
move_group_->move();
}
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();
RCLCPP_INFO(this->get_logger(), "GET START STATE");
auto robot_start_state = planning_components->getStartState();
void relativeMoveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) {
RCLCPP_INFO(this->get_logger(), "GOT MSG");
RCLCPP_INFO(this->get_logger(), "SETSTART TO CUR");
planning_components->setStartStateToCurrentState();
moveit::core::RobotStatePtr robot_state = move_group_->getCurrentState();
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
RCLCPP_INFO(this->get_logger(), "SET GOAL");
planning_components->setGoal(*p, "base_link");
geometry_msgs::msg::Pose target_pose;
target_pose.position.x = 0.24;
target_pose.position.y = -0.32;
target_pose.position.z = 0.56;
move_group_->setPoseTarget(target_pose);
// Actually plan
RCLCPP_INFO(this->get_logger(), "DO PLAN");
auto plan_solution = planning_components->plan();
moveit::planning_interface::MoveGroupInterface::Plan plan;
bool success = (move_group_->plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
// Plan is valid
if (plan_solution) {
// Follow through with the planned path
planning_components->execute();
}
RCLCPP_INFO(this->get_logger(), "Did work? %s", success ? "yes" : "no");
move_group_->move();
}
private:
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr relative_pose_sub_;
//please
//rclcpp::Node::SharedPtr nh_;
// moveit_cpp::MoveItCpp::SharedPtr moveit_cpp_ptr_;
// rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
// std::thread thread_executor_spin_;
moveit::planning_interface::MoveGroupInterfacePtr move_group_;
moveit::core::RobotModelConstPtr robot_model_;
};
int main(int argc, char* argv[]) {
@ -83,12 +84,9 @@ int main(int argc, char* argv[]) {
auto node = std::make_shared<ScanNPlan>();
RCLCPP_INFO(node->get_logger(), "MoveGroup node started.");
node->init();
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::shutdown();
return 0;