nic/drawing-robot-ros2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add freedrive xArm to target height

Browse Source
This commit is contained in:
Nicolas Hiillos
2023-03-30 14:34:58 +03:00
parent dc0445abca
commit ba13618c95
4 changed files with 102 additions and 248 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
src/lite6_controller/CMakeLists.txt
View File

@@ -50,6 +50,7 @@ ament_target_dependencies(lite6_controller
add_executable(lite6_calibration src/lite6_calibration.cpp)
ament_target_dependencies(lite6_calibration
"xarm_msgs"
"xarm_api"
"rclcpp"
"moveit"
"rclcpp_action"

126
src/lite6_controller/launch/lite6_real_calibration.launch.py
View File

@@ -1,126 +0,0 @@
import os
from launch import LaunchDescription
from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node
from ament_index_python import get_package_share_directory
from launch.launch_description_sources import load_python_launch_file_as_module
from launch import LaunchDescription
from launch.actions import RegisterEventHandler, EmitEvent
from launch.event_handlers import OnProcessExit
from launch.events import Shutdown
def launch_setup(context, *args, **kwargs):
robot_ip = LaunchConfiguration('robot_ip', default='192.168.1.150')
report_type = LaunchConfiguration('report_type', default='normal')
prefix = LaunchConfiguration('prefix', default='')
hw_ns = LaunchConfiguration('hw_ns', default='ufactory')
limited = LaunchConfiguration('limited', default=True)
effort_control = LaunchConfiguration('effort_control', default=False)
velocity_control = LaunchConfiguration('velocity_control', default=False)
add_gripper = LaunchConfiguration('add_gripper', default=False)
add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
dof = LaunchConfiguration('dof', default=6)
robot_type = LaunchConfiguration('robot_type', default='lite')
no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False)
add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
geometry_type = LaunchConfiguration('geometry_type', default='box')
geometry_mass = LaunchConfiguration('geometry_mass', default=0.1)
geometry_height = LaunchConfiguration('geometry_height', default=0.1)
geometry_radius = LaunchConfiguration('geometry_radius', default=0.1)
geometry_length = LaunchConfiguration('geometry_length', default=0.1)
geometry_width = LaunchConfiguration('geometry_width', default=0.1)
geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='')
geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"')
geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"')
geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"')
geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"')
baud_checkset = LaunchConfiguration('baud_checkset', default=True)
default_gripper_baud = LaunchConfiguration('default_gripper_baud', default=2000000)
ros2_control_plugin = 'uf_robot_hardware/UFRobotSystemHardware'
controllers_name = LaunchConfiguration('controllers_name', default='controllers')
moveit_controller_manager_key = LaunchConfiguration('moveit_controller_manager_key', default='moveit_simple_controller_manager')
moveit_controller_manager_value = LaunchConfiguration('moveit_controller_manager_value', default='moveit_simple_controller_manager/MoveItSimpleControllerManager')
xarm_type = '{}{}'.format(robot_type.perform(context), dof.perform(context))
ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context)
use_sim_time = LaunchConfiguration('use_sim_time', default=False)
log_level = LaunchConfiguration("log_level", default='warn')
# robot_description_parameters
# xarm_moveit_config/launch/lib/robot_moveit_config_lib.py
moveit_config_package_name = 'custom_xarm_moveit_config'
mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory(moveit_config_package_name), 'launch', 'lib', 'robot_moveit_config_lib.py'))
get_xarm_robot_description_parameters = getattr(mod, 'get_xarm_robot_description_parameters')
robot_description_parameters = get_xarm_robot_description_parameters(
xacro_urdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
xacro_srdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
urdf_arguments={
'prefix': prefix,
'hw_ns': hw_ns.perform(context).strip('/'),
'limited': limited,
'effort_control': effort_control,
'velocity_control': velocity_control,
'add_gripper': add_gripper,
'add_vacuum_gripper': add_vacuum_gripper,
'dof': dof,
'robot_type': robot_type,
'ros2_control_plugin': ros2_control_plugin,
'add_other_geometry': add_other_geometry,
'geometry_type': geometry_type,
'geometry_mass': geometry_mass,
'geometry_height': geometry_height,
'geometry_radius': geometry_radius,
'geometry_length': geometry_length,
'geometry_width': geometry_width,
'geometry_mesh_filename': geometry_mesh_filename,
'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz,
'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy,
'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz,
'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy,
},
srdf_arguments={
'prefix': prefix,
'dof': dof,
'robot_type': robot_type,
'add_gripper': add_gripper,
'add_vacuum_gripper': add_vacuum_gripper,
'add_other_geometry': add_other_geometry,
},
arguments={
'context': context,
'xarm_type': xarm_type,
}
)
calibration_node = Node(
package="lite6_controller",
executable="lite6_calibration",
output="screen",
arguments=["--ros-args", "--log-level", log_level],
parameters=[
robot_description_parameters,
{"use_sim_time": use_sim_time},
],
)
return [
RegisterEventHandler(event_handler=OnProcessExit(
target_action=calibration_node,
on_exit=[EmitEvent(event=Shutdown())]
)),
calibration_node
]
def generate_launch_description():
return LaunchDescription([
OpaqueFunction(function=launch_setup)
])

1
src/lite6_controller/package.xml
View File

@@ -16,6 +16,7 @@
<depend>moveit</depend>
<depend>moveit_msgs</depend>
<depend>xarm_msgs</depend>
<depend>xarm_api</depend>
<depend>geometry_msgs</depend>
<depend>moveit_ros_planning_interface</depend>
<depend>tf2_ros</depend>

222
src/lite6_controller/src/lite6_calibration.cpp
View File

@@ -1,128 +1,106 @@
#include <cstdio>
#include <rclcpp/rclcpp.hpp>
#include <robot_controller/robot_controller.hpp>
#include <signal.h>
#include <chrono>
#include <cstdlib>
#include <queue>
#include <fstream>
#include <rclcpp/rclcpp.hpp>
#include <xarm_api/xarm_ros_client.h>
#include <iostream>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <moveit_msgs/msg/collision_object.hpp>
//#include <moveit/planning_interface/planning_interface.h>
//#include <moveit/planning_interface/planning_response.h>
//#include <moveit/kinematic_constraints/kinematic_constraint.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_eigen/tf2_eigen.hpp>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/common_planning_interface_objects/common_objects.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <string>
#include <moveit_msgs/msg/constraints.hpp>
#include <moveit_msgs/msg/robot_trajectory.hpp>
#include <moveit_msgs/msg/motion_plan_request.hpp>
#include <moveit_msgs/msg/motion_sequence_request.hpp>
#include <moveit_msgs/msg/motion_sequence_item.hpp>
#include <moveit_msgs/srv/get_motion_sequence.hpp>
#include <moveit/moveit_cpp/moveit_cpp.h>
#include <moveit/moveit_cpp/planning_component.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <pilz_industrial_motion_planner/command_list_manager.h>
#include <xarm_msgs/srv/move_joint.hpp>
#include <xarm_msgs/srv/get_float32_list.hpp>
const std::string MOVE_GROUP = "lite6";
using namespace std::chrono_literals;
class Calibration : public rclcpp::Node
void exit_sig_handler(int signum)
{
public:
//moveit::planning_interface::MoveGroupInterface move_group;
rclcpp::TimerBase::SharedPtr status_timer;
rclcpp::Client<xarm_msgs::srv::MoveJoint>::SharedPtr set_joint_client;
rclcpp::Client<xarm_msgs::srv::GetFloat32List>::SharedPtr get_joint_client;
Calibration(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
: Node("lite6_calibration",options)
//move_group(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP)
{
//RCLCPP_INFO(this->get_logger(), "Starting state monitor");
//move_group.startStateMonitor(30);
//RCLCPP_INFO(this->get_logger(), "Started state monitor");
//move_group.setPlanningTime(30.0);
//status_timer = this->create_wall_timer(5s, std::bind(&Calibration::getTree, this));
RCLCPP_ERROR(this->get_logger(), "create service client");
set_joint_client = this->create_client<xarm_msgs::srv::MoveJoint>("/ufactory/set_servo_angle");
get_joint_client = this->create_client<xarm_msgs::srv::GetFloat32List>("/ufactory/get_servo_angle");
//xarm_msgs::srv::MoveJoint
while (!set_joint_client->wait_for_service(1s)) {
RCLCPP_ERROR(this->get_logger(), "service not available, waiting again...");
}
while (!get_joint_client->wait_for_service(1s)) {
RCLCPP_ERROR(this->get_logger(), "service not available, waiting again...");
}
RCLCPP_ERROR(this->get_logger(), "Client ready");
//RCLCPP_ERROR(this->get_logger(), "create request");
//auto request = std::make_shared<xarm_msgs::srv::MoveJoint::Request>();
//RCLCPP_ERROR(this->get_logger(), "send request");
//auto result = joint_client->async_send_request(request);
//RCLCPP_ERROR(this->get_logger(), "get request");
//auto a = result.get()->message;
//RCLCPP_ERROR(this->get_logger(), "A:%s", a.c_str());
auto request = std::make_shared<xarm_msgs::srv::GetFloat32List::Request>();
RCLCPP_ERROR(this->get_logger(), "");
auto result = get_joint_client->async_send_request(request);
auto msg = result.get()->message;
RCLCPP_ERROR(this->get_logger(), "A:%s", msg.c_str());
}
void getTree()
{
//rclcpp::sleep_for(20s);
RCLCPP_INFO(this->get_logger(), "Getting robot state");
//moveit::core::RobotStatePtr move_group_state = move_group.getCurrentState(30);
//RCLCPP_INFO(this->get_logger(), move_group_state->getStateTreeString().c_str());
//RCLCPP_ERROR(this->get_logger(), "AAAAAAAAAA");
}
};
/**
*
*/
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto calibration = std::make_shared<Calibration>();
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(calibration);
executor.spin();
rclcpp::shutdown();
return EXIT_SUCCESS;
fprintf(stderr, "[lite6_calibration] Ctrl-C caught, exit process...\n");
exit(-1);
}
void wait()
{
do
{
std::cout << '\n' << "Press a key to continue...";
} while (std::cin.get() != '\n');
}
void println(std::string s) {
std::cout << s << std::endl;
}
void print_joints(std::vector<float> jnts) {
for (auto i : jnts)
std::cout << i << std::endl;
}
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
std::string hw_ns = "ufactory";
std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("lite6_calibration");
int ret;
signal(SIGINT, exit_sig_handler);
xarm_api::XArmROSClient client;
client.init(node, hw_ns);
client.motion_enable(true);
println("Setting xArm lite6 to free-drive mode. Attach pen and touch the drawing surface.");
client.set_mode(2);
client.set_state(0);
//rclcpp::sleep_for(std::chrono::seconds(20));
wait();
//client.set_mode(0);
//client.set_state(0);
//rclcpp::sleep_for(std::chrono::seconds(2));
client.set_mode(0);
client.set_state(0);
rclcpp::sleep_for(std::chrono::seconds(2));
std::vector<float> jnt_drawing_pose = {-0.813972, -0.0103697, 0.554617, 3.09979, -0.627334, -0.397301, 0};
std::vector<float> jnt_pose = { 0, 0, 0, 0, 0, 0, 0 };
client.get_servo_angle(jnt_pose);
println("Moving to start drawing pose");
//XArmROSClient::set_servo_angle(const std::vector<fp32>& angles, fp32 speed, fp32 acc, fp32 mvtime, bool wait, fp32 timeout, fp32 radius)
client.set_servo_angle(jnt_drawing_pose, 1, 1, 1, true, 100, -1);
//client.set_servo_angle_j(jnt_drawing_pose, 1, 1, 100);
//rclcpp::sleep_for(std::chrono::seconds(5));
client.set_mode(0);
client.set_state(0);
return 0;
client.set_mode(4);
client.set_state(0);
std::vector<float> jnt_v = { 1, 0, 0, 0, 0, 0, 0 };
ret = client.vc_set_joint_velocity(jnt_v);
RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_joint: %d", ret);
rclcpp::sleep_for(std::chrono::seconds(2));
jnt_v[0] = -1;
ret = client.vc_set_joint_velocity(jnt_v);
RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_joint: %d", ret);
rclcpp::sleep_for(std::chrono::seconds(2));
// stop
jnt_v[0] = 0;
ret = client.vc_set_joint_velocity(jnt_v);
RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_joint: %d", ret);
std::vector<float> line_v = { 1, 0, 0, 0, 0, 0};
client.set_mode(5);
client.set_state(0);
ret = client.vc_set_cartesian_velocity(line_v);
RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_line: %d", ret);
rclcpp::sleep_for(std::chrono::seconds(2));
line_v[0] = -1;
ret = client.vc_set_cartesian_velocity(line_v);
RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_line: %d", ret);
rclcpp::sleep_for(std::chrono::seconds(2));
// stop
line_v[0] = 0;
ret = client.vc_set_cartesian_velocity(line_v);
RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_line: %d", ret);
RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "test_xarm_velo_move over");
return 0;
}