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

Facilitate xarm calibration in C++

Browse Source
This commit is contained in:
Nicolas Hiillos
2023-03-30 10:18:28 +03:00
parent 1c02d8dce2
commit 6467d80bc0
4 changed files with 270 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
src/lite6_controller/CMakeLists.txt
View File

@@ -16,6 +16,8 @@ find_package(moveit REQUIRED)
find_package(pilz_industrial_motion_planner REQUIRED) find_package(pilz_industrial_motion_planner REQUIRED)
find_package(moveit_msgs REQUIRED) find_package(moveit_msgs REQUIRED)
find_package(geometry_msgs REQUIRED) find_package(geometry_msgs REQUIRED)
find_package(xarm_api REQUIRED)
find_package(xarm_msgs REQUIRED)
find_package(tf2_ros REQUIRED) find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED) find_package(geometry_msgs REQUIRED)
@@ -45,6 +47,18 @@ ament_target_dependencies(lite6_controller
"moveit_ros_planning_interface" "moveit_ros_planning_interface"
"robot_interfaces") "robot_interfaces")
add_executable(lite6_calibration src/lite6_calibration.cpp)
ament_target_dependencies(lite6_calibration
"xarm_msgs"
"rclcpp"
"moveit"
"rclcpp_action"
"Eigen3"
"pilz_industrial_motion_planner"
"robot_controller"
"moveit_ros_planning_interface"
"robot_interfaces")
if(BUILD_TESTING) if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED) find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies() ament_lint_auto_find_test_dependencies()
@@ -52,6 +66,7 @@ endif()
install(TARGETS install(TARGETS
lite6_controller lite6_controller
lite6_calibration
DESTINATION lib/${PROJECT_NAME}) DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})

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

@@ -0,0 +1,126 @@
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

@@ -15,6 +15,7 @@
<!-- <depend>robot_controller</depend> --> <!-- <depend>robot_controller</depend> -->
<depend>moveit</depend> <depend>moveit</depend>
<depend>moveit_msgs</depend> <depend>moveit_msgs</depend>
<depend>xarm_msgs</depend>
<depend>geometry_msgs</depend> <depend>geometry_msgs</depend>
<depend>moveit_ros_planning_interface</depend> <depend>moveit_ros_planning_interface</depend>
<depend>tf2_ros</depend> <depend>tf2_ros</depend>

128
src/lite6_controller/src/lite6_calibration.cpp Normal file
View File

@@ -0,0 +1,128 @@
#include <cstdio>
#include <rclcpp/rclcpp.hpp>
#include <robot_controller/robot_controller.hpp>
#include <chrono>
#include <cstdlib>
#include <queue>
#include <fstream>
#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 <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
{
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;
}