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

res/req and vizualization not working reeeee

Browse source
This commit is contained in:
John Farrell 2022-02-24 09:32:50 -05:00
parent c6f05f0db3
commit 0f4886c48f
Signed by: john
GPG key ID: 53EF5A5459F750F5
4 changed files with 100 additions and 9 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -42,6 +42,16 @@ def generate_launch_description():
arguments=['0', '0', '0', '0', '0', '0', 'world', 'base_link']
),
Node(
package='move_group_interface',
executable='move_group_interface_ndde',
output='screen',
),
Node(
package='move_group_interface',
executable='client_node',
output='screen',
),
Node(
package='joystickmove',
executable='move',

18
src/move_group_interface/src/client_node.cpp
View file

@ -1,13 +1,19 @@
#include <rclcpp/rclcpp.hpp>
#include <move_group_interface/srv/move_pose.hpp>
#include "geometry_msgs/msg/pose_stamped.hpp"
class ScanNPlan : public rclcpp::Node {
public:
ScanNPlan() : Node("scan_n_plan") {
move_client_ = this->create_client<move_group_interface::srv::MovePose>("update_pose");
pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
"move_pose",
rclcpp::QoS(1),
std::bind(&ScanNPlan::moveCallback, this, std::placeholders::_1)
);
}
void start() {
void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr msg) {
RCLCPP_INFO(get_logger(), "Attempting to move to pose");
// Wait for service to be available
@ -18,7 +24,7 @@ public:
// Create a request for the MovePose service call
auto request = std::make_shared<move_group_interface::srv::MovePose::Request>();
request->pose = msg;
auto future = move_client_->async_send_request(request);
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) != rclcpp::executor::FutureReturnCode::SUCCESS) {
@ -32,6 +38,9 @@ public:
return;
}
// Successfully moved
/*
RCLCPP_INFO(this->get_logger(), "Moved to pose: x: %f, y: %f, z: %f - x: %f, y: %f, z: %f, w: %f",
response->pose.position.x,
response->pose.position.y,
@ -42,17 +51,20 @@ public:
response->pose.orientation.z,
response->pose.orientation.w
);
*/
}
private:
rclcpp::Client<move_group_interface::srv::MovePose>::SharedPtr move_client_;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
};
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
/*
auto app = std::make_shared<ScanNPlan>();
app->start();
*/
rclcpp::shutdown();
return 0;

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

@ -1,16 +1,31 @@
#include <rclcpp/rclcpp.hpp>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include <string>
// MoveitCpp
#include <moveit/moveit_cpp/moveit_cpp.h>
#include <moveit/moveit_cpp/planning_component.h>
// https://github.com/ros-planning/moveit_visual_tools/tree/foxy
// #include <moveit_visual_tools/moveit_visual_tools.h>
#include <move_group_interface/srv/move_pose.hpp>
#include "geometry_msgs/msg/pose_stamped.hpp"
// namespace rvt = rviz_visual_tools;
const std::string PLANNING_GROUP = "base_link";
class Localizer : public rclcpp::Node {
public:
Localizer() : Node("move_group_interface_node"), last_msg_{nullptr} {
Localizer() : Node("move_group_interface_node") {//, last_msg_{nullptr} {
/*
pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
"move_pose",
rclcpp::QoS(1),
std::bind(&Localizer::moveCallback, this, std::placeholders::_1)
);
*/
server_ = this->create_service<move_group_interface::srv::MovePose>(
"update_pose",
@ -18,14 +33,16 @@ class Localizer : public rclcpp::Node {
);
}
/*
void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr msg) {
last_msg_ = msg;
}
*/
void movePose(move_group_interface::srv::MovePose::Request::SharedPtr req,
move_group_interface::srv::MovePose::Response::SharedPtr res) {
// Read last message
geometry_msgs::msg::PoseStamped::SharedPtr p = last_msg_;
geometry_msgs::msg::PoseStamped::SharedPtr p = req->pose; // last_msg_;
if (!p) {
RCLCPP_ERROR(this->get_logger(), "no data");
@ -33,13 +50,64 @@ class Localizer : public rclcpp::Node {
return;
}
res->header = p->header;
// https://moveit.picknik.ai/galactic/doc/examples/moveit_cpp/moveitcpp_tutorial.html
// set up with current position
auto moveit_cpp_ptr = std::make_shared<moveit_cpp::MoveItCpp>(this);
moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService();
auto planning_components = std::make_shared<moveit_cpp::PlanningComponent>(PLANNING_GROUP, moveit_cpp_ptr);
auto robot_model_ptr = moveit_cpp_ptr->getRobotModel();
auto robot_start_state = planning_components->getStartState();
// auto joint_model_group_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP);
// set up visualizations
/*
moveit_visual_tools::MoveItVisualTools visual_tools(node, "base_link", "move_group_interface", moveit_cpp_ptr->getPlanningSceneMonitor());
visual_tools.deleteAllMarkers();
visual_tools.loadRemoteControl();
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools.trigger();
*/
// just in case
planning_components->setStartStateToCurrentState();
// set the goal to the request pose
planning_components->setGoal(p, "ar3_arm");
// actually plan
auto plan_solution = planning_components->plan();
// plan is valid
if (plan_solution) {
// display the robot in rviz
/*
visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("base_link"), "start_pose");
visual_tools.publishText(text_pose, "Start Pose", rvt::WHITE, rvt::XLARGE);
visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose");
visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(plan_solution.trajectory, joint_model_group_ptr);
visual_tools.trigger();
*/
// Execute plan
planning_components->execute();
}
// set response variables to updated robot states
res->header = p->header; // TODO: change these to current state (post move)
res->pose = p->pose;
res->success = true;
}
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
geometry_msgs::msg::PoseStamped::SharedPtr last_msg_;
// rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
// geometry_msgs::msg::PoseStamped::SharedPtr last_msg_;
rclcpp::Service<move_group_interface::srv::MovePose>::SharedPtr server_;
};

1
src/move_group_interface/srv/MovePose.srv
View file

@ -1,5 +1,6 @@
#request
string base_frame
geometry_msgs/PoseStamped pose
---
#response
std_msgs/Header header