Compare commits
No commits in common. "91a042ef9f99894932c711739da70eebd78594f0" and "2d858ab96152db5b952ab8b6db44e9e4f056ed7a" have entirely different histories.
91a042ef9f
...
2d858ab961
4 changed files with 46 additions and 70 deletions
|
@ -54,9 +54,7 @@ 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")),
|
||||
{ "default_planning_pipeline": "ompl" },
|
||||
{ "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }]
|
||||
get_yaml(join(config_root, "ar3_controllers.yaml"))],
|
||||
),
|
||||
|
||||
# Our joystick node
|
||||
|
@ -135,7 +133,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": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }
|
||||
{ "planning_pipelines": [ "ompl", "move_group"] }
|
||||
],
|
||||
),
|
||||
|
||||
|
|
|
@ -14,7 +14,6 @@ namespace ar3_hardware
|
|||
hardware_interface::return_type Ar3SystemHardware::configure(
|
||||
const hardware_interface::HardwareInfo & info)
|
||||
{
|
||||
// Initialize default configuration
|
||||
if (configure_default(info) != hardware_interface::return_type::OK)
|
||||
{
|
||||
return hardware_interface::return_type::ERROR;
|
||||
|
@ -22,15 +21,12 @@ hardware_interface::return_type Ar3SystemHardware::configure(
|
|||
|
||||
//hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
|
||||
//hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]);
|
||||
|
||||
// Resize position and command vectors to match actual configuration
|
||||
// This makes sure a default value is initialized for each joint and saves
|
||||
hw_positions_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
|
||||
hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
|
||||
|
||||
for (const hardware_interface::ComponentInfo & joint : info_.joints)
|
||||
{
|
||||
// We only use one command interface
|
||||
// DiffBotSystem has exactly two states and one command interface on each joint
|
||||
if (joint.command_interfaces.size() != 1)
|
||||
{
|
||||
RCLCPP_FATAL(
|
||||
|
@ -40,7 +36,6 @@ hardware_interface::return_type Ar3SystemHardware::configure(
|
|||
return hardware_interface::return_type::ERROR;
|
||||
}
|
||||
|
||||
// Make sure the command interface is in position mode
|
||||
if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION)
|
||||
{
|
||||
RCLCPP_FATAL(
|
||||
|
@ -50,7 +45,6 @@ hardware_interface::return_type Ar3SystemHardware::configure(
|
|||
return hardware_interface::return_type::ERROR;
|
||||
}
|
||||
|
||||
// We only publish to one state interface
|
||||
if (joint.state_interfaces.size() != 1)
|
||||
{
|
||||
RCLCPP_FATAL(
|
||||
|
@ -60,7 +54,6 @@ hardware_interface::return_type Ar3SystemHardware::configure(
|
|||
return hardware_interface::return_type::ERROR;
|
||||
}
|
||||
|
||||
// Make sure the state interface is in position mode
|
||||
if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION)
|
||||
{
|
||||
RCLCPP_FATAL(
|
||||
|
@ -78,7 +71,6 @@ hardware_interface::return_type Ar3SystemHardware::configure(
|
|||
|
||||
std::vector<hardware_interface::StateInterface> Ar3SystemHardware::export_state_interfaces()
|
||||
{
|
||||
// Tell the controller about all joints that we publish state info for
|
||||
std::vector<hardware_interface::StateInterface> state_interfaces;
|
||||
for (auto i = 0u; i < info_.joints.size(); i++)
|
||||
{
|
||||
|
@ -91,7 +83,6 @@ std::vector<hardware_interface::StateInterface> Ar3SystemHardware::export_state_
|
|||
|
||||
std::vector<hardware_interface::CommandInterface> Ar3SystemHardware::export_command_interfaces()
|
||||
{
|
||||
// Tell the controller about all joints that we take commands from
|
||||
std::vector<hardware_interface::CommandInterface> command_interfaces;
|
||||
for (auto i = 0u; i < info_.joints.size(); i++)
|
||||
{
|
||||
|
@ -116,12 +107,6 @@ hardware_interface::return_type Ar3SystemHardware::start()
|
|||
}
|
||||
}
|
||||
|
||||
// Hack to make sure the node stays running by putting it in its own thread.
|
||||
// this is because we wrote the hardware interface in python earlier and
|
||||
// the driver just listens on some subscribers. This allows a node in this
|
||||
// controller to publish values to that driver
|
||||
// The correct thing to do would be to move all of the driver code into this
|
||||
// controller and remove the node, publishers, and subscribers entierly
|
||||
executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
|
||||
nh_ = std::make_shared<rclcpp::Node>("ar3_hardware");
|
||||
executor_->add_node(nh_);
|
||||
|
@ -133,8 +118,6 @@ hardware_interface::return_type Ar3SystemHardware::start()
|
|||
};
|
||||
thread_executor_spin_ = std::thread(spin);
|
||||
|
||||
// Create joint state publishers
|
||||
// TODO: Don't use Float32, use Pose
|
||||
j1_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j1", 10);
|
||||
j2_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j2", 10);
|
||||
j3_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j3", 10);
|
||||
|
@ -142,8 +125,6 @@ hardware_interface::return_type Ar3SystemHardware::start()
|
|||
j5_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j5", 10);
|
||||
j6_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j6", 10);
|
||||
|
||||
// Create joint command subscribers
|
||||
// TODO: Don't use Float32, use Pose
|
||||
s_j1 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j1", 10, std::bind(&Ar3SystemHardware::update_j1_cb, this, std::placeholders::_1));
|
||||
s_j2 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j2", 10, std::bind(&Ar3SystemHardware::update_j2_cb, this, std::placeholders::_1));
|
||||
s_j3 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j3", 10, std::bind(&Ar3SystemHardware::update_j3_cb, this, std::placeholders::_1));
|
||||
|
@ -169,10 +150,6 @@ hardware_interface::return_type Ar3SystemHardware::stop()
|
|||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
// These callbacks are called from the command subscribers, they update
|
||||
// the hw_positions_ array with the current joint state that will be
|
||||
// sent to the controller.
|
||||
// Again, this is wrong and these values should be filled in by the read() function
|
||||
void Ar3SystemHardware::update_j1_cb(std_msgs::msg::Float32::SharedPtr msg) {
|
||||
hw_positions_[0] = msg->data;
|
||||
}
|
||||
|
@ -224,7 +201,6 @@ hardware_interface::return_type ar3_hardware::Ar3SystemHardware::write()
|
|||
{
|
||||
//RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Writing...");
|
||||
|
||||
// Publish joint commands to the arm driver
|
||||
auto message = std_msgs::msg::Float32();
|
||||
message.data = hw_commands_[0];
|
||||
j1_pub_->publish(message);
|
||||
|
|
|
@ -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 / 20, self._dosomething)
|
||||
self._timer = self.create_timer(1 / 40, self._dosomething)
|
||||
|
||||
# Set joint angles independantly
|
||||
base_topic = 'move/'
|
||||
|
|
|
@ -5,9 +5,8 @@
|
|||
#include <thread>
|
||||
|
||||
// MoveitCpp
|
||||
#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 <moveit/moveit_cpp/moveit_cpp.h>
|
||||
#include <moveit/moveit_cpp/planning_component.h>
|
||||
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
|
||||
|
@ -16,66 +15,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)
|
||||
);
|
||||
|
||||
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"));
|
||||
RCLCPP_INFO(this->get_logger(), "SUB EXIST");
|
||||
}
|
||||
|
||||
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");
|
||||
|
||||
move_group_->setPoseTarget(p->pose);
|
||||
//auto psm = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
|
||||
//psm->startSceneMonitor();
|
||||
|
||||
moveit::planning_interface::MoveGroupInterface::Plan plan;
|
||||
bool success = (move_group_->plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
|
||||
// ??????
|
||||
moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService();
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Did work? %s", success ? "yes" : "no");
|
||||
|
||||
move_group_->move();
|
||||
}
|
||||
// planning_scene_monitor::LockedPlanningSceneRW scene{moveit_cpp_ptr->getPlanningSceneMonitor()};
|
||||
RCLCPP_INFO(this->get_logger(), "NEW PLANNING");
|
||||
|
||||
void relativeMoveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) {
|
||||
RCLCPP_INFO(this->get_logger(), "GOT MSG");
|
||||
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();
|
||||
|
||||
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");
|
||||
RCLCPP_INFO(this->get_logger(), "SETSTART TO CUR");
|
||||
planning_components->setStartStateToCurrentState();
|
||||
|
||||
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);
|
||||
// Set the goal to the request pose
|
||||
RCLCPP_INFO(this->get_logger(), "SET GOAL");
|
||||
planning_components->setGoal(*p, "base_link");
|
||||
|
||||
moveit::planning_interface::MoveGroupInterface::Plan plan;
|
||||
bool success = (move_group_->plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
|
||||
// Actually plan
|
||||
RCLCPP_INFO(this->get_logger(), "DO PLAN");
|
||||
auto plan_solution = planning_components->plan();
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Did work? %s", success ? "yes" : "no");
|
||||
|
||||
move_group_->move();
|
||||
// Plan is valid
|
||||
if (plan_solution) {
|
||||
// Follow through with the planned path
|
||||
planning_components->execute();
|
||||
}
|
||||
}
|
||||
private:
|
||||
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
|
||||
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr relative_pose_sub_;
|
||||
|
||||
moveit::planning_interface::MoveGroupInterfacePtr move_group_;
|
||||
moveit::core::RobotModelConstPtr robot_model_;
|
||||
//please
|
||||
//rclcpp::Node::SharedPtr nh_;
|
||||
// moveit_cpp::MoveItCpp::SharedPtr moveit_cpp_ptr_;
|
||||
// rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
|
||||
// std::thread thread_executor_spin_;
|
||||
};
|
||||
|
||||
int main(int argc, char* argv[]) {
|
||||
|
@ -84,9 +83,12 @@ 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;
|
||||
|
|
Loading…
Reference in a new issue