add rm_arm_control
This commit is contained in:
hivecore
@@ -1,156 +0,0 @@
|
||||
import os
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
def generate_launch_description():
|
||||
# 获取包的共享目录
|
||||
driver_pkg_dir = get_package_share_directory('driver_pkg')
|
||||
perception_pkg_dir = get_package_share_directory('perception_pkg')
|
||||
decision_pkg_dir = get_package_share_directory('decision_pkg')
|
||||
control_pkg_dir = get_package_share_directory('control_pkg')
|
||||
interaction_pkg_dir = get_package_share_directory('interaction_pkg')
|
||||
|
||||
# 配置文件路径
|
||||
nav2_config = os.path.join(control_pkg_dir, 'config', 'nav2_config.yaml')
|
||||
|
||||
return LaunchDescription([
|
||||
# ========== 驱动层节点 ==========
|
||||
# 电机驱动
|
||||
Node(
|
||||
package='driver_pkg',
|
||||
executable='motor_driver_node',
|
||||
name='motor_driver',
|
||||
output='screen'
|
||||
),
|
||||
|
||||
# 激光雷达驱动
|
||||
Node(
|
||||
package='driver_pkg',
|
||||
executable='lidar_driver_node',
|
||||
name='lidar_driver',
|
||||
output='screen'
|
||||
),
|
||||
|
||||
# 相机驱动
|
||||
Node(
|
||||
package='driver_pkg',
|
||||
executable='camera_driver_node',
|
||||
name='camera_driver',
|
||||
output='screen'
|
||||
),
|
||||
|
||||
# ========== 感知层节点 ==========
|
||||
# 点云处理
|
||||
Node(
|
||||
package='perception_pkg',
|
||||
executable='point_cloud_processor',
|
||||
name='point_cloud_processor',
|
||||
output='screen'
|
||||
),
|
||||
|
||||
# 目标检测
|
||||
Node(
|
||||
package='perception_pkg',
|
||||
executable='object_detection_node.py',
|
||||
name='object_detection',
|
||||
output='screen'
|
||||
),
|
||||
|
||||
# SLAM (使用Cartographer)
|
||||
Node(
|
||||
package='cartographer_ros',
|
||||
executable='cartographer_node',
|
||||
name='cartographer_node',
|
||||
output='screen',
|
||||
arguments=[
|
||||
'-configuration_directory', os.path.join(perception_pkg_dir, 'config'),
|
||||
'-configuration_basename', 'cartographer_config.lua'
|
||||
]
|
||||
),
|
||||
|
||||
Node(
|
||||
package='cartographer_ros',
|
||||
executable='occupancy_grid_node',
|
||||
name='occupancy_grid_node',
|
||||
output='screen',
|
||||
arguments=['-resolution', '0.05']
|
||||
),
|
||||
|
||||
# ========== 决策层节点 ==========
|
||||
# 行为树
|
||||
Node(
|
||||
package='decision_pkg',
|
||||
executable='behavior_tree_node.py',
|
||||
name='behavior_tree',
|
||||
output='screen'
|
||||
),
|
||||
|
||||
# 任务规划
|
||||
Node(
|
||||
package='decision_pkg',
|
||||
executable='task_planner_node.py',
|
||||
name='task_planner',
|
||||
output='screen'
|
||||
),
|
||||
|
||||
# 路径规划 (使用Nav2)
|
||||
Node(
|
||||
package='nav2_bringup',
|
||||
executable='nav2_lifecycle_manager',
|
||||
name='lifecycle_manager_navigation',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'autostart': True,
|
||||
'node_names': [
|
||||
'controller_server',
|
||||
'planner_server',
|
||||
'recoveries_server',
|
||||
'bt_navigator',
|
||||
'waypoint_follower'
|
||||
]
|
||||
}]
|
||||
),
|
||||
|
||||
Node(
|
||||
package='nav2_planner',
|
||||
executable='planner_server',
|
||||
name='planner_server',
|
||||
output='screen',
|
||||
parameters=[nav2_config]
|
||||
),
|
||||
|
||||
Node(
|
||||
package='nav2_controller',
|
||||
executable='controller_server',
|
||||
name='controller_server',
|
||||
output='screen',
|
||||
parameters=[nav2_config]
|
||||
),
|
||||
|
||||
# ========== 控制层节点 ==========
|
||||
# 底盘控制器
|
||||
Node(
|
||||
package='control_pkg',
|
||||
executable='base_controller',
|
||||
name='base_controller',
|
||||
output='screen'
|
||||
),
|
||||
|
||||
# ========== 交互层节点 ==========
|
||||
# 状态监控
|
||||
Node(
|
||||
package='interaction_pkg',
|
||||
executable='robot_monitor.py',
|
||||
name='robot_monitor',
|
||||
output='screen'
|
||||
),
|
||||
|
||||
# Web桥接器
|
||||
Node(
|
||||
package='rosbridge_server',
|
||||
executable='rosbridge_websocket',
|
||||
name='rosbridge_websocket',
|
||||
output='screen'
|
||||
)
|
||||
])
|
||||
@@ -1,62 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(control_pkg)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclpy REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(trajectory_msgs REQUIRED)
|
||||
find_package(control_msgs REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(tf2 REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
# 添加可执行文件
|
||||
add_executable(arm_controller_node src/arm_controller.cpp)
|
||||
ament_target_dependencies(arm_controller_node
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
trajectory_msgs
|
||||
control_msgs
|
||||
rclcpp_action
|
||||
nav_msgs
|
||||
tf2 # 添加TF2依赖
|
||||
tf2_ros # 添加TF2 ROS依赖
|
||||
)
|
||||
|
||||
# 添加可执行文件
|
||||
add_executable(base_controller_node src/base_controller.cpp)
|
||||
ament_target_dependencies(base_controller_node
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
nav_msgs
|
||||
tf2 # 添加TF2依赖
|
||||
tf2_ros # 添加TF2 ROS依赖
|
||||
)
|
||||
|
||||
# 安装可执行文件
|
||||
install(TARGETS
|
||||
arm_controller_node
|
||||
base_controller_node
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
ament_package()
|
||||
@@ -1,24 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>control_pkg</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="ray@todo.todo">ray</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclpy</depend>
|
||||
<depend>control_msgs</depend>
|
||||
<depend>rclcpp_action</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -1,200 +0,0 @@
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sensor_msgs/msg/joint_state.hpp"
|
||||
#include "trajectory_msgs/msg/joint_trajectory.hpp"
|
||||
#include "control_msgs/action/follow_joint_trajectory.hpp"
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <chrono>
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory;
|
||||
using GoalHandleFollowJointTrajectory = rclcpp_action::ClientGoalHandle<FollowJointTrajectory>;
|
||||
|
||||
class ArmController : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
ArmController() : Node("arm_controller")
|
||||
{
|
||||
// 声明参数
|
||||
this->declare_parameter<std::vector<std::string>>("joint_names",
|
||||
std::vector<std::string>{"shoulder_pan_joint", "shoulder_lift_joint",
|
||||
"elbow_joint", "wrist_1_joint",
|
||||
"wrist_2_joint", "wrist_3_joint"});
|
||||
|
||||
// 获取关节名称
|
||||
joint_names_ = this->get_parameter("joint_names").as_string_array();
|
||||
|
||||
// 创建关节状态发布者
|
||||
joint_state_pub_ = this->create_publisher<sensor_msgs::msg::JointState>(
|
||||
"joint_states", 10);
|
||||
|
||||
// 创建轨迹动作客户端
|
||||
trajectory_client_ = rclcpp_action::create_client<FollowJointTrajectory>(
|
||||
this, "follow_joint_trajectory");
|
||||
|
||||
// 创建定时器，定期发布关节状态
|
||||
timer_ = this->create_wall_timer(
|
||||
10ms, std::bind(&ArmController::publishJointStates, this));
|
||||
|
||||
// 初始化关节状态
|
||||
initJointStates();
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Arm controller initialized");
|
||||
}
|
||||
|
||||
// 执行关节轨迹
|
||||
void executeTrajectory(const std::vector<std::vector<double>>& waypoints,
|
||||
const std::vector<double>& time_points)
|
||||
{
|
||||
// 等待动作服务器可用
|
||||
if (!trajectory_client_->wait_for_action_server(10s)) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
|
||||
return;
|
||||
}
|
||||
|
||||
// 创建目标
|
||||
auto goal_msg = FollowJointTrajectory::Goal();
|
||||
goal_msg.trajectory.joint_names = joint_names_;
|
||||
|
||||
// 添加轨迹点
|
||||
for (size_t i = 0; i < waypoints.size(); ++i) {
|
||||
trajectory_msgs::msg::JointTrajectoryPoint point;
|
||||
point.positions = waypoints[i];
|
||||
|
||||
// 设置时间戳
|
||||
rclcpp::Duration time_from_start = rclcpp::Duration::from_seconds(time_points[i]);
|
||||
builtin_interfaces::msg::Duration duration_msg;
|
||||
rclcpp::Duration rclcpp_duration = rclcpp::Duration::from_seconds(time_points[i]);
|
||||
|
||||
// 设置秒和纳秒部分
|
||||
duration_msg.sec = rclcpp_duration.seconds();
|
||||
duration_msg.nanosec = rclcpp_duration.nanoseconds() % 1000000000;
|
||||
|
||||
point.time_from_start = duration_msg;
|
||||
|
||||
goal_msg.trajectory.points.push_back(point);
|
||||
}
|
||||
|
||||
// 发送目标
|
||||
auto send_goal_options = rclcpp_action::Client<FollowJointTrajectory>::SendGoalOptions();
|
||||
send_goal_options.feedback_callback =
|
||||
[this](GoalHandleFollowJointTrajectory::SharedPtr,
|
||||
const std::shared_ptr<const FollowJointTrajectory::Feedback> feedback) {
|
||||
RCLCPP_INFO(this->get_logger(), "Current position: %.2f, %.2f, %.2f",
|
||||
feedback->actual.positions[0],
|
||||
feedback->actual.positions[1],
|
||||
feedback->actual.positions[2]);
|
||||
};
|
||||
|
||||
send_goal_options.result_callback =
|
||||
[this](const GoalHandleFollowJointTrajectory::WrappedResult& result) {
|
||||
switch (result.code) {
|
||||
case rclcpp_action::ResultCode::SUCCEEDED:
|
||||
RCLCPP_INFO(this->get_logger(), "Trajectory execution succeeded");
|
||||
break;
|
||||
case rclcpp_action::ResultCode::ABORTED:
|
||||
RCLCPP_ERROR(this->get_logger(), "Trajectory execution aborted");
|
||||
return;
|
||||
case rclcpp_action::ResultCode::CANCELED:
|
||||
RCLCPP_INFO(this->get_logger(), "Trajectory execution canceled");
|
||||
return;
|
||||
default:
|
||||
RCLCPP_ERROR(this->get_logger(), "Unknown result code");
|
||||
return;
|
||||
}
|
||||
};
|
||||
|
||||
auto goal_handle_future = trajectory_client_->async_send_goal(goal_msg, send_goal_options);
|
||||
|
||||
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), goal_handle_future) !=
|
||||
rclcpp::FutureReturnCode::SUCCESS) {
|
||||
RCLCPP_ERROR(this->get_logger(), "send goal call failed :(");
|
||||
return;
|
||||
}
|
||||
|
||||
goal_handle_ = goal_handle_future.get();
|
||||
if (!goal_handle_) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
|
||||
return;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
|
||||
}
|
||||
|
||||
// 简单的运动示例：将机械臂移动到预设位置
|
||||
void moveToHomePosition()
|
||||
{
|
||||
// 定义路径点（关节角度）
|
||||
std::vector<std::vector<double>> waypoints;
|
||||
std::vector<double> time_points;
|
||||
|
||||
// 当前位置（假设）
|
||||
waypoints.push_back(current_positions_);
|
||||
time_points.push_back(0.0);
|
||||
|
||||
// 目标位置（示例）
|
||||
std::vector<double> home_positions = {0.0, -1.57, 1.57, 0.0, 0.0, 0.0};
|
||||
waypoints.push_back(home_positions);
|
||||
time_points.push_back(3.0); // 3秒内到达
|
||||
|
||||
// 执行轨迹
|
||||
executeTrajectory(waypoints, time_points);
|
||||
}
|
||||
|
||||
private:
|
||||
void initJointStates()
|
||||
{
|
||||
// 初始化关节状态
|
||||
joint_state_msg_.header.frame_id = "base_link";
|
||||
joint_state_msg_.name = joint_names_;
|
||||
|
||||
// 初始化关节位置（全部为0）
|
||||
current_positions_.resize(joint_names_.size(), 0.0);
|
||||
joint_state_msg_.position = current_positions_;
|
||||
|
||||
// 初始化速度和加速度
|
||||
joint_state_msg_.velocity.resize(joint_names_.size(), 0.0);
|
||||
joint_state_msg_.effort.resize(joint_names_.size(), 0.0);
|
||||
}
|
||||
|
||||
void publishJointStates()
|
||||
{
|
||||
// 更新时间戳
|
||||
joint_state_msg_.header.stamp = this->now();
|
||||
|
||||
// 发布关节状态
|
||||
joint_state_pub_->publish(joint_state_msg_);
|
||||
}
|
||||
|
||||
// 关节状态发布者
|
||||
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_pub_;
|
||||
|
||||
// 轨迹动作客户端
|
||||
rclcpp_action::Client<FollowJointTrajectory>::SharedPtr trajectory_client_;
|
||||
GoalHandleFollowJointTrajectory::SharedPtr goal_handle_;
|
||||
|
||||
// 定时器
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
|
||||
// 关节状态消息
|
||||
sensor_msgs::msg::JointState joint_state_msg_;
|
||||
|
||||
// 关节名称和当前位置
|
||||
std::vector<std::string> joint_names_;
|
||||
std::vector<double> current_positions_;
|
||||
};
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<ArmController>();
|
||||
|
||||
// 示例：移动到预设位置
|
||||
node->moveToHomePosition();
|
||||
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -1,118 +0,0 @@
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "nav_msgs/msg/odometry.hpp"
|
||||
#include "tf2/LinearMath/Quaternion.h"
|
||||
#include "tf2_ros/transform_broadcaster.h"
|
||||
|
||||
class BaseController : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
BaseController() : Node("base_controller")
|
||||
{
|
||||
// 订阅参考速度
|
||||
cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
|
||||
"cmd_vel", 10, std::bind(&BaseController::cmdVelCallback, this, std::placeholders::_1));
|
||||
|
||||
// 发布里程计
|
||||
odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
|
||||
|
||||
// 创建TF广播器
|
||||
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
|
||||
|
||||
// 初始化状态
|
||||
x_ = 0.0;
|
||||
y_ = 0.0;
|
||||
theta_ = 0.0;
|
||||
|
||||
// 创建定时器，定期更新控制和发布状态
|
||||
timer_ = this->create_wall_timer(
|
||||
std::chrono::milliseconds(20), std::bind(&BaseController::controlLoop, this));
|
||||
}
|
||||
|
||||
private:
|
||||
void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
|
||||
{
|
||||
// 存储目标速度
|
||||
target_vx_ = msg->linear.x;
|
||||
target_vy_ = msg->linear.y;
|
||||
target_w_ = msg->angular.z;
|
||||
}
|
||||
|
||||
void controlLoop()
|
||||
{
|
||||
// 简单PID控制器实现
|
||||
double dt = 0.02; // 20ms控制周期
|
||||
|
||||
// 模拟速度控制
|
||||
current_vx_ = target_vx_; // 简化模型，实际应包含PID控制
|
||||
current_vy_ = target_vy_;
|
||||
current_w_ = target_w_;
|
||||
|
||||
// 更新位姿
|
||||
double delta_x = (current_vx_ * cos(theta_) - current_vy_ * sin(theta_)) * dt;
|
||||
double delta_y = (current_vx_ * sin(theta_) + current_vy_ * cos(theta_)) * dt;
|
||||
double delta_theta = current_w_ * dt;
|
||||
|
||||
x_ += delta_x;
|
||||
y_ += delta_y;
|
||||
theta_ += delta_theta;
|
||||
|
||||
// 发布TF变换
|
||||
geometry_msgs::msg::TransformStamped transform_stamped;
|
||||
transform_stamped.header.stamp = this->now();
|
||||
transform_stamped.header.frame_id = "odom";
|
||||
transform_stamped.child_frame_id = "base_link";
|
||||
|
||||
transform_stamped.transform.translation.x = x_;
|
||||
transform_stamped.transform.translation.y = y_;
|
||||
transform_stamped.transform.translation.z = 0.0;
|
||||
|
||||
tf2::Quaternion q;
|
||||
q.setRPY(0, 0, theta_);
|
||||
transform_stamped.transform.rotation.x = q.x();
|
||||
transform_stamped.transform.rotation.y = q.y();
|
||||
transform_stamped.transform.rotation.z = q.z();
|
||||
transform_stamped.transform.rotation.w = q.w();
|
||||
|
||||
tf_broadcaster_->sendTransform(transform_stamped);
|
||||
|
||||
// 发布里程计消息
|
||||
auto odom = std::make_unique<nav_msgs::msg::Odometry>();
|
||||
odom->header.stamp = this->now();
|
||||
odom->header.frame_id = "odom";
|
||||
odom->child_frame_id = "base_link";
|
||||
|
||||
odom->pose.pose.position.x = x_;
|
||||
odom->pose.pose.position.y = y_;
|
||||
odom->pose.pose.position.z = 0.0;
|
||||
odom->pose.pose.orientation.x = q.x();
|
||||
odom->pose.pose.orientation.y = q.y();
|
||||
odom->pose.pose.orientation.z = q.z();
|
||||
odom->pose.pose.orientation.w = q.w();
|
||||
|
||||
odom->twist.twist.linear.x = current_vx_;
|
||||
odom->twist.twist.linear.y = current_vy_;
|
||||
odom->twist.twist.angular.z = current_w_;
|
||||
|
||||
odom_pub_->publish(std::move(odom));
|
||||
}
|
||||
|
||||
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
|
||||
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
|
||||
// 机器人状态
|
||||
double x_, y_, theta_;
|
||||
double target_vx_, target_vy_, target_w_;
|
||||
double current_vx_, current_vy_, current_w_;
|
||||
};
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<BaseController>();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -1,25 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(decision_pkg)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclpy REQUIRED)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
@@ -1,21 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>decision_pkg</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="ray@todo.todo">ray</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclpy</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -1,108 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from py_trees.behaviour import Behaviour
|
||||
from py_trees.common import Status
|
||||
from py_trees.composites import Sequence, Selector
|
||||
from py_trees.trees import BehaviourTree
|
||||
from nav2_msgs.action import NavigateToPose
|
||||
from rclpy.action import ActionClient
|
||||
|
||||
class MoveBaseAction(Behaviour):
|
||||
def __init__(self, name, goal_pose):
|
||||
super(MoveBaseAction, self).__init__(name)
|
||||
self.goal_pose = goal_pose
|
||||
self.action_client = None
|
||||
self.goal_handle = None
|
||||
self.feedback = None
|
||||
self.result_future = None
|
||||
|
||||
def setup(self, **kwargs):
|
||||
node = kwargs.get('node')
|
||||
self.action_client = ActionClient(node, NavigateToPose, 'navigate_to_pose')
|
||||
self.action_client.wait_for_server()
|
||||
return True
|
||||
|
||||
def initialise(self):
|
||||
goal_msg = NavigateToPose.Goal()
|
||||
goal_msg.pose = self.goal_pose
|
||||
|
||||
self.goal_handle = self.action_client.send_goal_async(
|
||||
goal_msg, feedback_callback=self.feedback_callback)
|
||||
|
||||
self.result_future = self.goal_handle.add_done_callback(self.goal_response_callback)
|
||||
|
||||
def update(self):
|
||||
if self.result_future.done():
|
||||
goal_status = self.goal_handle.result().status
|
||||
if goal_status == 4: # SUCCEEDED
|
||||
return Status.SUCCESS
|
||||
else:
|
||||
return Status.FAILURE
|
||||
return Status.RUNNING
|
||||
|
||||
def terminate(self, new_status):
|
||||
if self.action_client.is_server_ready() and self.goal_handle is not None:
|
||||
self.action_client.cancel_goal_async(self.goal_handle)
|
||||
|
||||
def goal_response_callback(self, future):
|
||||
goal_handle = future.result()
|
||||
if not goal_handle.accepted:
|
||||
self.logger.error('Goal rejected by server')
|
||||
return
|
||||
|
||||
self.logger.info('Goal accepted by server, waiting for result')
|
||||
self.result_future = goal_handle.get_result_async()
|
||||
self.result_future.add_done_callback(self.get_result_callback)
|
||||
|
||||
def get_result_callback(self, future):
|
||||
result = future.result().result
|
||||
self.logger.info(f'Navigation result: {result}')
|
||||
|
||||
def feedback_callback(self, feedback_msg):
|
||||
self.feedback = feedback_msg.feedback
|
||||
# 处理反馈信息
|
||||
|
||||
class BehaviorTreeNode(Node):
|
||||
def __init__(self):
|
||||
super().__init__('behavior_tree_node')
|
||||
|
||||
# 创建行为树
|
||||
self.create_behavior_tree()
|
||||
|
||||
# 创建定时器，定期执行行为树
|
||||
self.timer = self.create_timer(0.1, self.execute_behavior_tree)
|
||||
|
||||
self.get_logger().info('Behavior tree node initialized')
|
||||
|
||||
def create_behavior_tree(self):
|
||||
# 定义目标位置
|
||||
goal_pose1 = ... # 填充目标位姿
|
||||
goal_pose2 = ...
|
||||
|
||||
# 创建行为
|
||||
move_to_kitchen = MoveBaseAction("Move to Kitchen", goal_pose1)
|
||||
move_to_living_room = MoveBaseAction("Move to Living Room", goal_pose2)
|
||||
|
||||
# 创建序列
|
||||
sequence = Sequence("Main Sequence")
|
||||
sequence.add_children([move_to_kitchen, move_to_living_room])
|
||||
|
||||
# 创建行为树
|
||||
self.tree = BehaviourTree(sequence)
|
||||
|
||||
# 设置节点
|
||||
self.tree.setup(node=self)
|
||||
|
||||
def execute_behavior_tree(self):
|
||||
# 执行行为树
|
||||
self.tree.tick()
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = BehaviorTreeNode()
|
||||
rclpy.spin(node)
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,65 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(driver_pkg)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(OpenCV REQUIRED)
|
||||
find_package(serial_driver REQUIRED)
|
||||
find_package(cv_bridge REQUIRED)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
# 添加电机驱动可执行文件
|
||||
add_executable(motor_driver_node src/motor_driver_node.cpp)
|
||||
ament_target_dependencies(motor_driver_node
|
||||
rclcpp
|
||||
geometry_msgs
|
||||
sensor_msgs
|
||||
serial_driver
|
||||
OpenCV # 添加OpenCV依赖
|
||||
cv_bridge # 添加cv_bridge依赖
|
||||
)
|
||||
|
||||
# 添加激光雷达驱动可执行文件
|
||||
add_executable(lidar_driver_node src/lidar_driver_node.cpp)
|
||||
ament_target_dependencies(lidar_driver_node
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
serial_driver
|
||||
)
|
||||
|
||||
# 添加相机驱动可执行文件
|
||||
add_executable(camera_driver_node src/camera_driver_node.cpp)
|
||||
ament_target_dependencies(camera_driver_node
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
serial_driver
|
||||
OpenCV # 添加OpenCV依赖
|
||||
cv_bridge # 添加cv_bridge依赖
|
||||
)
|
||||
|
||||
# 安装可执行文件
|
||||
install(TARGETS
|
||||
motor_driver_node
|
||||
lidar_driver_node
|
||||
camera_driver_node
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
ament_package()
|
||||
@@ -1,27 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>driver_pkg</name>
|
||||
<version>0.0.0</version>
|
||||
<description>Robot driver package</description>
|
||||
<maintainer email="ray@hivecore.cn">ray</maintainer>
|
||||
<license>Apache License 2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>serial_driver</depend>
|
||||
<depend>OpenCV</depend>
|
||||
<depend>cv_bridge</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -1,145 +0,0 @@
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sensor_msgs/msg/image.hpp"
|
||||
#include "cv_bridge/cv_bridge.h"
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include "serial/serial.h"
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <chrono>
|
||||
|
||||
class CameraDriverNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
CameraDriverNode() : Node("camera_driver_node")
|
||||
{
|
||||
// 声明并获取参数
|
||||
this->declare_parameter<int>("camera_id", 0);
|
||||
this->declare_parameter<std::string>("frame_id", "camera_frame");
|
||||
this->declare_parameter<int>("width", 640);
|
||||
this->declare_parameter<int>("height", 480);
|
||||
this->declare_parameter<double>("fps", 30.0);
|
||||
|
||||
camera_id_ = this->get_parameter("camera_id").as_int();
|
||||
frame_id_ = this->get_parameter("frame_id").as_string();
|
||||
width_ = this->get_parameter("width").as_int();
|
||||
height_ = this->get_parameter("height").as_int();
|
||||
fps_ = this->get_parameter("fps").as_double();
|
||||
|
||||
// 创建图像消息发布者
|
||||
image_pub_ = this->create_publisher<sensor_msgs::msg::Image>("image_raw", 10);
|
||||
|
||||
// 初始化相机
|
||||
if (!initCamera()) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to initialize camera");
|
||||
return;
|
||||
}
|
||||
|
||||
// 创建图像采集线程
|
||||
image_thread_ = std::thread(&CameraDriverNode::imageCaptureLoop, this);
|
||||
RCLCPP_INFO(this->get_logger(), "Camera driver node initialized");
|
||||
}
|
||||
|
||||
~CameraDriverNode()
|
||||
{
|
||||
// 停止图像采集线程
|
||||
running_ = false;
|
||||
if (image_thread_.joinable()) {
|
||||
image_thread_.join();
|
||||
}
|
||||
|
||||
// 释放相机资源
|
||||
if (cap_.isOpened()) {
|
||||
cap_.release();
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
bool initCamera()
|
||||
{
|
||||
// 打开相机
|
||||
cap_.open(camera_id_);
|
||||
|
||||
if (!cap_.isOpened()) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Unable to open camera %d", camera_id_);
|
||||
return false;
|
||||
}
|
||||
|
||||
// 设置相机参数
|
||||
cap_.set(cv::CAP_PROP_FRAME_WIDTH, width_);
|
||||
cap_.set(cv::CAP_PROP_FRAME_HEIGHT, height_);
|
||||
cap_.set(cv::CAP_PROP_FPS, fps_);
|
||||
|
||||
// 检查参数设置是否成功
|
||||
int actual_width = static_cast<int>(cap_.get(cv::CAP_PROP_FRAME_WIDTH));
|
||||
int actual_height = static_cast<int>(cap_.get(cv::CAP_PROP_FRAME_HEIGHT));
|
||||
double actual_fps = cap_.get(cv::CAP_PROP_FPS);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Camera parameters: width=%d, height=%d, fps=%.1f",
|
||||
actual_width, actual_height, actual_fps);
|
||||
|
||||
// 捕获一帧测试
|
||||
cv::Mat test_frame;
|
||||
if (!cap_.read(test_frame)) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to capture test frame from camera");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void imageCaptureLoop()
|
||||
{
|
||||
running_ = true;
|
||||
rclcpp::Rate rate(fps_);
|
||||
|
||||
while (running_ && rclcpp::ok()) {
|
||||
// 捕获图像
|
||||
cv::Mat frame;
|
||||
if (cap_.read(frame)) {
|
||||
// 转换为ROS图像消息
|
||||
auto image_msg = cv_bridge::CvImage(
|
||||
std_msgs::msg::Header(),
|
||||
"bgr8",
|
||||
frame
|
||||
).toImageMsg();
|
||||
|
||||
// 设置时间戳和坐标系
|
||||
image_msg->header.stamp = this->now();
|
||||
image_msg->header.frame_id = frame_id_;
|
||||
|
||||
// 发布图像消息
|
||||
image_pub_->publish(*image_msg);
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(), "Failed to capture frame from camera");
|
||||
}
|
||||
|
||||
rate.sleep();
|
||||
}
|
||||
}
|
||||
|
||||
// 相机对象
|
||||
cv::VideoCapture cap_;
|
||||
|
||||
// 参数
|
||||
int camera_id_;
|
||||
std::string frame_id_;
|
||||
int width_;
|
||||
int height_;
|
||||
double fps_;
|
||||
|
||||
// 发布者
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;
|
||||
|
||||
// 图像采集线程
|
||||
std::thread image_thread_;
|
||||
bool running_{false};
|
||||
};
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<CameraDriverNode>();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -1,214 +0,0 @@
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sensor_msgs/msg/laser_scan.hpp"
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <thread>
|
||||
#include <chrono>
|
||||
|
||||
class LidarDriverNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
LidarDriverNode() : Node("lidar_driver_node")
|
||||
{
|
||||
// 声明并获取参数
|
||||
this->declare_parameter<std::string>("port", "/dev/ttyUSB0");
|
||||
this->declare_parameter<int>("baudrate", 115200);
|
||||
this->declare_parameter<std::string>("frame_id", "laser_frame");
|
||||
this->declare_parameter<double>("frequency", 10.0);
|
||||
|
||||
port_ = this->get_parameter("port").as_string();
|
||||
baudrate_ = this->get_parameter("baudrate").as_int();
|
||||
frame_id_ = this->get_parameter("frame_id").as_string();
|
||||
frequency_ = this->get_parameter("frequency").as_double();
|
||||
|
||||
// 创建激光扫描消息发布者
|
||||
scan_pub_ = this->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
|
||||
|
||||
// 初始化LIDAR
|
||||
if (!initLidar()) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to initialize LIDAR");
|
||||
return;
|
||||
}
|
||||
|
||||
// 创建数据采集线程
|
||||
data_thread_ = std::thread(&LidarDriverNode::dataCollectionLoop, this);
|
||||
RCLCPP_INFO(this->get_logger(), "LIDAR driver node initialized");
|
||||
}
|
||||
|
||||
~LidarDriverNode()
|
||||
{
|
||||
// 停止数据采集线程
|
||||
running_ = false;
|
||||
if (data_thread_.joinable()) {
|
||||
data_thread_.join();
|
||||
}
|
||||
|
||||
// 关闭串口
|
||||
if (serial_port_.isOpen()) {
|
||||
serial_port_.close();
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
bool initLidar()
|
||||
{
|
||||
try {
|
||||
// 配置串口
|
||||
serial_port_.setPort(port_);
|
||||
serial_port_.setBaudrate(baudrate_);
|
||||
serial::Timeout timeout = serial::Timeout::simpleTimeout(1000);
|
||||
serial_port_.setTimeout(timeout);
|
||||
|
||||
// 打开串口
|
||||
serial_port_.open();
|
||||
|
||||
if (serial_port_.isOpen()) {
|
||||
RCLCPP_INFO(this->get_logger(), "Successfully connected to LIDAR on port %s", port_.c_str());
|
||||
|
||||
// 发送初始化命令（根据具体LIDAR型号调整）
|
||||
std::string init_cmd = "start_scan\n";
|
||||
serial_port_.write(init_cmd);
|
||||
|
||||
// 等待LIDAR初始化
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
return true;
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "Unable to open port %s", port_.c_str());
|
||||
return false;
|
||||
}
|
||||
} catch (serial::SerialException& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Serial exception: %s", e.what());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void dataCollectionLoop()
|
||||
{
|
||||
running_ = true;
|
||||
rclcpp::Rate rate(frequency_);
|
||||
|
||||
// 激光扫描消息初始化
|
||||
auto scan_msg = std::make_unique<sensor_msgs::msg::LaserScan>();
|
||||
scan_msg->header.frame_id = frame_id_;
|
||||
|
||||
// 假设LIDAR参数（根据实际LIDAR型号调整）
|
||||
scan_msg->angle_min = -3.141592; // -180度
|
||||
scan_msg->angle_max = 3.141592; // 180度
|
||||
scan_msg->angle_increment = 0.0174533; // 约1度
|
||||
scan_msg->time_increment = 0.0;
|
||||
scan_msg->scan_time = 1.0 / frequency_;
|
||||
scan_msg->range_min = 0.1; // 最小检测距离10厘米
|
||||
scan_msg->range_max = 30.0; // 最大检测距离30米
|
||||
|
||||
// 存储激光点数量
|
||||
size_t num_points = static_cast<size_t>((scan_msg->angle_max - scan_msg->angle_min) / scan_msg->angle_increment) + 1;
|
||||
scan_msg->ranges.resize(num_points);
|
||||
scan_msg->intensities.resize(num_points);
|
||||
|
||||
while (running_ && rclcpp::ok()) {
|
||||
try {
|
||||
// 读取LIDAR数据（根据具体LIDAR协议解析）
|
||||
if (serial_port_.available()) {
|
||||
std::string data = serial_port_.read(serial_port_.available());
|
||||
|
||||
// 解析数据（示例，实际需根据LIDAR协议实现）
|
||||
if (parseLidarData(data, scan_msg->ranges, scan_msg->intensities)) {
|
||||
// 设置时间戳
|
||||
scan_msg->header.stamp = this->now();
|
||||
|
||||
// 发布激光扫描消息
|
||||
scan_pub_->publish(std::move(scan_msg));
|
||||
|
||||
// 为下一次发布创建新消息
|
||||
scan_msg = std::make_unique<sensor_msgs::msg::LaserScan>();
|
||||
scan_msg->header.frame_id = frame_id_;
|
||||
scan_msg->angle_min = -3.141592;
|
||||
scan_msg->angle_max = 3.141592;
|
||||
scan_msg->angle_increment = 0.0174533;
|
||||
scan_msg->time_increment = 0.0;
|
||||
scan_msg->scan_time = 1.0 / frequency_;
|
||||
scan_msg->range_min = 0.1;
|
||||
scan_msg->range_max = 30.0;
|
||||
scan_msg->ranges.resize(num_points);
|
||||
scan_msg->intensities.resize(num_points);
|
||||
}
|
||||
}
|
||||
} catch (serial::SerialException& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Serial exception during data collection: %s", e.what());
|
||||
running_ = false;
|
||||
}
|
||||
|
||||
rate.sleep();
|
||||
}
|
||||
}
|
||||
|
||||
bool parseLidarData(const std::string& data, std::vector<float>& ranges, std::vector<float>& intensities)
|
||||
{
|
||||
// 这里需要根据具体LIDAR的数据协议实现解析逻辑
|
||||
// 示例代码仅作为框架，实际应用中需要替换为真实的解析逻辑
|
||||
|
||||
// 简单示例：假设数据格式为"RANGE1,INTENSITY1;RANGE2,INTENSITY2;..."
|
||||
// 注意：这只是一个示例，实际LIDAR协议会复杂得多
|
||||
|
||||
size_t pos = 0;
|
||||
size_t point_index = 0;
|
||||
std::string token;
|
||||
|
||||
while ((pos = data.find(';')) != std::string::npos && point_index < ranges.size()) {
|
||||
token = data.substr(0, pos);
|
||||
|
||||
// 解析距离和强度
|
||||
size_t comma_pos = token.find(',');
|
||||
if (comma_pos != std::string::npos) {
|
||||
try {
|
||||
float range = std::stof(token.substr(0, comma_pos));
|
||||
float intensity = std::stof(token.substr(comma_pos + 1));
|
||||
|
||||
// 检查范围
|
||||
if (range >= 0.1 && range <= 30.0) {
|
||||
ranges[point_index] = range;
|
||||
intensities[point_index] = intensity;
|
||||
} else {
|
||||
ranges[point_index] = std::numeric_limits<float>::infinity();
|
||||
intensities[point_index] = 0.0;
|
||||
}
|
||||
} catch (const std::invalid_argument&) {
|
||||
// 解析失败，设置为无穷大
|
||||
ranges[point_index] = std::numeric_limits<float>::infinity();
|
||||
intensities[point_index] = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
data.erase(0, pos + 1);
|
||||
point_index++;
|
||||
}
|
||||
|
||||
// 如果解析到了足够的点，返回true
|
||||
return point_index > ranges.size() / 2;
|
||||
}
|
||||
|
||||
// 串口通信对象
|
||||
serial::Serial serial_port_;
|
||||
|
||||
// 参数
|
||||
std::string port_;
|
||||
int baudrate_;
|
||||
std::string frame_id_;
|
||||
double frequency_;
|
||||
|
||||
// 发布者
|
||||
rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan_pub_;
|
||||
|
||||
// 数据采集线程
|
||||
std::thread data_thread_;
|
||||
bool running_{false};
|
||||
};
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<LidarDriverNode>();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -1,75 +0,0 @@
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "sensor_msgs/msg/joint_state.hpp"
|
||||
|
||||
class MotorDriverNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
MotorDriverNode() : Node("motor_driver_node")
|
||||
{
|
||||
// 订阅速度命令
|
||||
cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
|
||||
"cmd_vel", 10, std::bind(&MotorDriverNode::cmdVelCallback, this, std::placeholders::_1));
|
||||
|
||||
// 发布关节状态
|
||||
joint_state_pub_ = this->create_publisher<sensor_msgs::msg::JointState>(
|
||||
"joint_states", 10);
|
||||
|
||||
// 初始化硬件接口
|
||||
initHardwareInterface();
|
||||
|
||||
// 创建定时器，定期发布关节状态
|
||||
timer_ = this->create_wall_timer(
|
||||
std::chrono::milliseconds(50), std::bind(&MotorDriverNode::publishJointStates, this));
|
||||
}
|
||||
|
||||
private:
|
||||
void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
|
||||
{
|
||||
// 将ROS速度命令转换为电机PWM值
|
||||
double linear = msg->linear.x;
|
||||
double angular = msg->angular.z;
|
||||
|
||||
// 控制电机
|
||||
setMotorSpeeds(linear, angular);
|
||||
}
|
||||
|
||||
void publishJointStates()
|
||||
{
|
||||
// 读取电机编码器值
|
||||
auto joint_state = std::make_unique<sensor_msgs::msg::JointState>();
|
||||
joint_state->header.stamp = this->now();
|
||||
// 填充关节状态数据
|
||||
// ...
|
||||
|
||||
joint_state_pub_->publish(std::move(joint_state));
|
||||
}
|
||||
|
||||
void initHardwareInterface()
|
||||
{
|
||||
// 初始化电机控制器接口
|
||||
// ...
|
||||
}
|
||||
|
||||
void setMotorSpeeds(double linear, double angular)
|
||||
{
|
||||
// 实现差速驱动转换
|
||||
// ...
|
||||
|
||||
// 发送PWM信号到电机
|
||||
// ...
|
||||
}
|
||||
|
||||
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_pub_;
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
};
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<MotorDriverNode>();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -1,25 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(interaction_pkg)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclpy REQUIRED)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
@@ -1,21 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>interaction_pkg</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="ray@todo.todo">ray</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclpy</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -1,133 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import BatteryState, LaserScan
|
||||
from nav_msgs.msg import Odometry
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib.animation import FuncAnimation
|
||||
import numpy as np
|
||||
|
||||
class RobotMonitor(Node):
|
||||
def __init__(self):
|
||||
super().__init__('robot_monitor')
|
||||
|
||||
# 订阅电池状态
|
||||
self.battery_sub = self.create_subscription(
|
||||
BatteryState, 'battery_state', self.battery_callback, 10)
|
||||
|
||||
# 订阅激光扫描
|
||||
self.laser_sub = self.create_subscription(
|
||||
LaserScan, 'scan', self.laser_callback, 10)
|
||||
|
||||
# 订阅里程计
|
||||
self.odom_sub = self.create_subscription(
|
||||
Odometry, 'odom', self.odom_callback, 10)
|
||||
|
||||
# 初始化数据
|
||||
self.battery_voltage = 0.0
|
||||
self.laser_ranges = []
|
||||
self.robot_x = 0.0
|
||||
self.robot_y = 0.0
|
||||
|
||||
# 创建图形
|
||||
self.fig, (self.ax1, self.ax2, self.ax3) = plt.subplots(3, 1, figsize=(10, 15))
|
||||
self.fig.suptitle('Robot Status Monitor')
|
||||
|
||||
# 设置电池电量图
|
||||
self.battery_bar = self.ax1.bar(['Battery'], [0], color='green')
|
||||
self.ax1.set_ylim(0, 100)
|
||||
self.ax1.set_ylabel('Battery Level (%)')
|
||||
|
||||
# 设置激光扫描图
|
||||
self.laser_line, = self.ax2.plot([], [], 'b-')
|
||||
self.ax2.set_xlim(-10, 10)
|
||||
self.ax2.set_ylim(-10, 10)
|
||||
self.ax2.set_aspect('equal')
|
||||
self.ax2.set_title('Laser Scan')
|
||||
|
||||
# 设置轨迹图
|
||||
self.trajectory_line, = self.ax3.plot([], [], 'r-')
|
||||
self.robot_marker, = self.ax3.plot([], [], 'bo', markersize=10)
|
||||
self.ax3.set_xlim(-5, 5)
|
||||
self.ax3.set_ylim(-5, 5)
|
||||
self.ax3.set_aspect('equal')
|
||||
self.ax3.set_title('Robot Trajectory')
|
||||
|
||||
# 轨迹数据
|
||||
self.trajectory_x = []
|
||||
self.trajectory_y = []
|
||||
|
||||
# 创建动画
|
||||
self.ani = FuncAnimation(self.fig, self.update_plot, interval=100)
|
||||
|
||||
self.get_logger().info('Robot monitor initialized')
|
||||
|
||||
def battery_callback(self, msg):
|
||||
# 假设电压范围为10.0-12.6V
|
||||
voltage = msg.voltage
|
||||
self.battery_voltage = max(0.0, min(100.0, (voltage - 10.0) / (12.6 - 10.0) * 100.0))
|
||||
|
||||
def laser_callback(self, msg):
|
||||
self.laser_ranges = msg.ranges
|
||||
self.laser_angles = np.linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
|
||||
|
||||
def odom_callback(self, msg):
|
||||
self.robot_x = msg.pose.pose.position.x
|
||||
self.robot_y = msg.pose.pose.position.y
|
||||
self.trajectory_x.append(self.robot_x)
|
||||
self.trajectory_y.append(self.robot_y)
|
||||
|
||||
# 限制轨迹长度
|
||||
if len(self.trajectory_x) > 1000:
|
||||
self.trajectory_x = self.trajectory_x[-1000:]
|
||||
self.trajectory_y = self.trajectory_y[-1000:]
|
||||
|
||||
def update_plot(self, frame):
|
||||
# 更新电池图
|
||||
self.battery_bar[0].set_height(self.battery_voltage)
|
||||
if self.battery_voltage < 20:
|
||||
self.battery_bar[0].set_color('red')
|
||||
elif self.battery_voltage < 50:
|
||||
self.battery_bar[0].set_color('yellow')
|
||||
else:
|
||||
self.battery_bar[0].set_color('green')
|
||||
|
||||
# 更新激光图
|
||||
if len(self.laser_ranges) > 0:
|
||||
x = np.array(self.laser_ranges) * np.cos(self.laser_angles)
|
||||
y = np.array(self.laser_ranges) * np.sin(self.laser_angles)
|
||||
self.laser_line.set_data(x, y)
|
||||
|
||||
# 更新轨迹图
|
||||
self.trajectory_line.set_data(self.trajectory_x, self.trajectory_y)
|
||||
self.robot_marker.set_data(self.robot_x, self.robot_y)
|
||||
|
||||
# 调整坐标轴范围以适应轨迹
|
||||
if len(self.trajectory_x) > 0:
|
||||
x_min, x_max = min(self.trajectory_x), max(self.trajectory_x)
|
||||
y_min, y_max = min(self.trajectory_y), max(self.trajectory_y)
|
||||
margin = 1.0
|
||||
self.ax3.set_xlim(min(x_min - margin, -5), max(x_max + margin, 5))
|
||||
self.ax3.set_ylim(min(y_min - margin, -5), max(y_max + margin, 5))
|
||||
|
||||
return self.battery_bar, self.laser_line, self.trajectory_line, self.robot_marker
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = RobotMonitor()
|
||||
|
||||
# 在单独的线程中运行ROS节点
|
||||
import threading
|
||||
thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
|
||||
thread.start()
|
||||
|
||||
# 显示图形
|
||||
plt.tight_layout()
|
||||
plt.show()
|
||||
|
||||
# 关闭节点
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
thread.join()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,65 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(preception_pkg)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# 设置CMake策略以处理_ROOT变量
|
||||
cmake_policy(SET CMP0074 NEW)
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclpy REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(OpenCV REQUIRED)
|
||||
# 查找PCL和ROS2 PCL包
|
||||
find_package(PCL REQUIRED COMPONENTS common filters features io segmentation surface)
|
||||
find_package(pcl_conversions REQUIRED)
|
||||
find_package(pcl_ros REQUIRED)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
# 添加C++组件
|
||||
add_executable(point_cloud_processor src/point_cloud_processor.cpp)
|
||||
ament_target_dependencies(point_cloud_processor
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
pcl_conversions
|
||||
pcl_ros
|
||||
)
|
||||
|
||||
# 链接PCL库
|
||||
target_link_libraries(point_cloud_processor
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
|
||||
# 安装C++可执行文件
|
||||
install(TARGETS
|
||||
point_cloud_processor
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
# 安装Python脚本
|
||||
install(PROGRAMS
|
||||
scripts/object_detection_node.py
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
# 安装其他资源
|
||||
install(DIRECTORY
|
||||
models
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@@ -1,25 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>preception_pkg</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="ray@todo.todo">ray</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclpy</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>pcl_conversions</depend>
|
||||
<depend>pcl_ros</depend>
|
||||
<depend>PCL</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -1,91 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import Image
|
||||
from cv_bridge import CvBridge
|
||||
import cv2
|
||||
import numpy as np
|
||||
import tensorflow as tf
|
||||
|
||||
class ObjectDetectionNode(Node):
|
||||
def __init__(self):
|
||||
super().__init__('object_detection_node')
|
||||
|
||||
# 创建图像订阅者
|
||||
self.image_sub = self.create_subscription(
|
||||
Image, 'camera/image_raw', self.image_callback, 10)
|
||||
|
||||
# 创建检测结果发布者
|
||||
self.detection_pub = self.create_publisher(Image, 'detection_image', 10)
|
||||
|
||||
# 初始化OpenCV桥接器
|
||||
self.bridge = CvBridge()
|
||||
|
||||
# 加载TensorFlow模型
|
||||
self.model = tf.saved_model.load('path/to/saved_model')
|
||||
|
||||
# 获取类别名称
|
||||
self.class_names = ['person', 'bicycle', 'car', ...] # 根据实际模型调整
|
||||
|
||||
self.get_logger().info('Object detection node initialized')
|
||||
|
||||
def image_callback(self, msg):
|
||||
try:
|
||||
# 将ROS图像消息转换为OpenCV格式
|
||||
cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
|
||||
|
||||
# 预处理图像
|
||||
input_tensor = tf.convert_to_tensor(cv_image)
|
||||
input_tensor = input_tensor[tf.newaxis, ...]
|
||||
|
||||
# 运行目标检测
|
||||
detections = self.model(input_tensor)
|
||||
|
||||
# 处理检测结果
|
||||
num_detections = int(detections.pop('num_detections'))
|
||||
detections = {key: value[0, :num_detections].numpy()
|
||||
for key, value in detections.items()}
|
||||
detections['num_detections'] = num_detections
|
||||
|
||||
# 过滤置信度低的检测结果
|
||||
detections['detection_classes'] = detections['detection_classes'].astype(np.int64)
|
||||
scores = detections['detection_scores']
|
||||
classes = detections['detection_classes']
|
||||
boxes = detections['detection_boxes']
|
||||
|
||||
# 在图像上绘制检测结果
|
||||
for i in range(len(scores)):
|
||||
if scores[i] > 0.5: # 置信度阈值
|
||||
class_id = classes[i]
|
||||
class_name = self.class_names[class_id]
|
||||
box = boxes[i]
|
||||
|
||||
# 转换边界框坐标
|
||||
h, w, _ = cv_image.shape
|
||||
ymin, xmin, ymax, xmax = box
|
||||
xmin = int(xmin * w)
|
||||
xmax = int(xmax * w)
|
||||
ymin = int(ymin * h)
|
||||
ymax = int(ymax * h)
|
||||
|
||||
# 绘制边界框和标签
|
||||
cv2.rectangle(cv_image, (xmin, ymin), (xmax, ymax), (0, 255, 0), 2)
|
||||
cv2.putText(cv_image, f'{class_name}: {scores[i]:.2f}',
|
||||
(xmin, ymin - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
|
||||
|
||||
# 发布结果图像
|
||||
output_msg = self.bridge.cv2_to_imgmsg(cv_image, 'bgr8')
|
||||
output_msg.header = msg.header
|
||||
self.detection_pub.publish(output_msg)
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Error processing image: {str(e)}')
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = ObjectDetectionNode()
|
||||
rclpy.spin(node)
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,52 +0,0 @@
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sensor_msgs/msg/point_cloud2.hpp"
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <pcl/filters/voxel_grid.h>
|
||||
|
||||
class PointCloudProcessor : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
PointCloudProcessor() : Node("point_cloud_processor")
|
||||
{
|
||||
// 订阅原始点云
|
||||
cloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
|
||||
"raw_cloud", 10, std::bind(&PointCloudProcessor::cloudCallback, this, std::placeholders::_1));
|
||||
|
||||
// 发布处理后的点云
|
||||
filtered_cloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(
|
||||
"filtered_cloud", 10);
|
||||
}
|
||||
|
||||
private:
|
||||
void cloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
|
||||
{
|
||||
// 转换为PCL格式
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
pcl::fromROSMsg(*msg, *cloud);
|
||||
|
||||
// 应用体素网格滤波降采样
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
pcl::VoxelGrid<pcl::PointXYZ> sor;
|
||||
sor.setInputCloud(cloud);
|
||||
sor.setLeafSize(0.01f, 0.01f, 0.01f);
|
||||
sor.filter(*cloud_filtered);
|
||||
|
||||
// 转换回ROS格式并发布
|
||||
sensor_msgs::msg::PointCloud2 output;
|
||||
pcl::toROSMsg(*cloud_filtered, output);
|
||||
output.header = msg->header;
|
||||
filtered_cloud_pub_->publish(output);
|
||||
}
|
||||
|
||||
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_sub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr filtered_cloud_pub_;
|
||||
};
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<PointCloudProcessor>();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
69
HiveCoreR1/src/rm_arm_control/CMakeLists.txt
Normal file
69
HiveCoreR1/src/rm_arm_control/CMakeLists.txt
Normal file
@@ -0,0 +1,69 @@
|
||||
cmake_minimum_required(VERSION 3.10)
|
||||
project(rm_arm_control)
|
||||
|
||||
# 设置 C 标准
|
||||
set(CMAKE_C_STANDARD 11)
|
||||
|
||||
# 添加忽略定义重定义警告的选项
|
||||
if(CMAKE_C_COMPILER_ID STREQUAL "GNU" OR CMAKE_C_COMPILER_ID STREQUAL "Clang")
|
||||
add_compile_options(-Wno-define-redefinition)
|
||||
endif()
|
||||
|
||||
# 依赖
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(Eigen3 REQUIRED) # 查找Eigen3库
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(trajectory_msgs REQUIRED)
|
||||
find_package(control_msgs REQUIRED)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
${EIGEN3_INCLUDE_DIRS} # 引入Eigen3的头文件路径
|
||||
)
|
||||
|
||||
|
||||
# 设置源文件
|
||||
set(SOURCES
|
||||
src/rm_arm_handler.cpp
|
||||
src/rm_arm_node.cpp
|
||||
)
|
||||
|
||||
# 添加可执行文件
|
||||
add_executable(rm_arm_control ${SOURCES})
|
||||
|
||||
ament_target_dependencies(rm_arm_control
|
||||
rclcpp
|
||||
geometry_msgs
|
||||
sensor_msgs
|
||||
trajectory_msgs
|
||||
control_msgs
|
||||
)
|
||||
|
||||
# 查找库
|
||||
find_library(RMAN_API_LIB NAMES api_cpp PATHS "${CMAKE_CURRENT_SOURCE_DIR}/Robotic_Arm/lib")
|
||||
|
||||
|
||||
# 链接库
|
||||
target_link_libraries(rm_arm_control ${RMAN_API_LIB})
|
||||
# 包含目录
|
||||
target_include_directories(rm_arm_control PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/Robotic_Arm/include")
|
||||
|
||||
# 如果是 UNIX 平台且不是 ARM 平台，确保动态库文件可以在运行时被找到（这一步通常不是必须的，但在某些配置下可能需要）
|
||||
if(UNIX)
|
||||
add_custom_command(TARGET rm_arm_control POST_BUILD
|
||||
COMMAND ${CMAKE_COMMAND} -E copy_if_different
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/Robotic_Arm/lib/libapi_cpp.so"
|
||||
$<TARGET_FILE_DIR:rm_arm_control>)
|
||||
endif()
|
||||
|
||||
install(TARGETS
|
||||
rm_arm_control
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
install(DIRECTORY launch
|
||||
DESTINATION share/${PROJECT_NAME}/
|
||||
)
|
||||
|
||||
ament_package()
|
||||
13
HiveCoreR1/src/rm_arm_control/LICENSE
Normal file
13
HiveCoreR1/src/rm_arm_control/LICENSE
Normal file
@@ -0,0 +1,13 @@
|
||||
Copyright 2024 realman-robotics
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
1035
HiveCoreR1/src/rm_arm_control/Robotic_Arm/include/rm_define.h
Normal file
1035
HiveCoreR1/src/rm_arm_control/Robotic_Arm/include/rm_define.h
Normal file
File diff suppressed because it is too large
Load Diff
4533
HiveCoreR1/src/rm_arm_control/Robotic_Arm/include/rm_interface.h
Normal file
4533
HiveCoreR1/src/rm_arm_control/Robotic_Arm/include/rm_interface.h
Normal file
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,16 @@
|
||||
#ifndef RM_INTERFACE_GLOBAL_H
|
||||
#define RM_INTERFACE_GLOBAL_H
|
||||
|
||||
#ifdef __linux
|
||||
#define RM_INTERFACE_EXPORT
|
||||
#endif
|
||||
|
||||
#if _WIN32
|
||||
#if defined(RM_INTERFACE_LIBRARY)
|
||||
# define RM_INTERFACE_EXPORT __declspec(dllexport)
|
||||
#else
|
||||
# define RM_INTERFACE_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#endif // RM_INTERFACE_GLOBAL_H
|
||||
4553
HiveCoreR1/src/rm_arm_control/Robotic_Arm/include/rm_service.h
Normal file
4553
HiveCoreR1/src/rm_arm_control/Robotic_Arm/include/rm_service.h
Normal file
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,16 @@
|
||||
|
||||
#ifndef RM_VERSION_H
|
||||
#define RM_VERSION_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#define SDK_VERSION ("1.1.1")
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
BIN
HiveCoreR1/src/rm_arm_control/Robotic_Arm/lib/api_cpp.dll
Normal file
BIN
HiveCoreR1/src/rm_arm_control/Robotic_Arm/lib/api_cpp.dll
Normal file
Binary file not shown.
BIN
HiveCoreR1/src/rm_arm_control/Robotic_Arm/lib/api_cpp.lib
Normal file
BIN
HiveCoreR1/src/rm_arm_control/Robotic_Arm/lib/api_cpp.lib
Normal file
Binary file not shown.
BIN
HiveCoreR1/src/rm_arm_control/Robotic_Arm/lib/libapi_cpp.so
Normal file
BIN
HiveCoreR1/src/rm_arm_control/Robotic_Arm/lib/libapi_cpp.so
Normal file
Binary file not shown.
3600
HiveCoreR1/src/rm_arm_control/data/ECO65_canfd_data.txt
Normal file
3600
HiveCoreR1/src/rm_arm_control/data/ECO65_canfd_data.txt
Normal file
File diff suppressed because it is too large
Load Diff
3600
HiveCoreR1/src/rm_arm_control/data/RM65&RM63_canfd_data.txt
Normal file
3600
HiveCoreR1/src/rm_arm_control/data/RM65&RM63_canfd_data.txt
Normal file
File diff suppressed because it is too large
Load Diff
3600
HiveCoreR1/src/rm_arm_control/data/RM75_canfd_data.txt
Normal file
3600
HiveCoreR1/src/rm_arm_control/data/RM75_canfd_data.txt
Normal file
File diff suppressed because it is too large
Load Diff
29
HiveCoreR1/src/rm_arm_control/data/robot_log.txt
Normal file
29
HiveCoreR1/src/rm_arm_control/data/robot_log.txt
Normal file
@@ -0,0 +1,29 @@
|
||||
[20240708 17:56:47] Begin API Testing.
|
||||
[20240708 18:05:04] Begin API Testing.
|
||||
[20240708 18:08:31] Begin API Testing.
|
||||
[20240708 18:33:17] Begin API Testing.
|
||||
[20240708 19:21:46] Begin API Testing.
|
||||
[20240708 19:59:38] Begin API Testing.
|
||||
[20240710 14:04:26] Begin API Testing.
|
||||
[20240710 14:07:27] Begin API Testing.
|
||||
[20240710 14:17:49] Begin API Testing.
|
||||
[20240710 14:22:08] Begin API Testing.
|
||||
[20240710 14:31:25] Begin API Testing.
|
||||
[20240710 18:16:24] Begin API Testing.
|
||||
[20240711 08:01:43] Begin API Testing.
|
||||
[20240711 09:44:12] Begin API Testing.
|
||||
[20240711 10:21:41] Begin API Testing.
|
||||
[20240711 10:30:04] Begin API Testing.
|
||||
[20240711 10:30:38] Begin API Testing.
|
||||
[20240711 14:53:33] Begin API Testing.
|
||||
[20240711 19:00:44] Begin API Testing.
|
||||
[20240711 19:03:09] Begin API Testing.
|
||||
[20240711 19:05:39] Begin API Testing.
|
||||
[20240712 09:52:46] Begin API Testing.
|
||||
[20240712 17:47:12] Begin API Testing.
|
||||
[20240712 17:47:50] Begin API Testing.
|
||||
[20240712 17:50:39] Begin API Testing.
|
||||
[20240712 17:55:20] Begin API Testing.
|
||||
[20240713 18:14:24] Begin API Testing.
|
||||
[20240713 18:17:21] Begin API Testing.
|
||||
[20240713 18:18:33] Begin API Testing.
|
||||
582
HiveCoreR1/src/rm_arm_control/data/trajectory.txt
Normal file
582
HiveCoreR1/src/rm_arm_control/data/trajectory.txt
Normal file
@@ -0,0 +1,582 @@
|
||||
{"point":[-81,-16537,15047,-5480,-17,17705,-17]}
|
||||
{"point":[-83,-16537,15048,-5484,-12,17707,-17]}
|
||||
{"point":[-82,-16537,15054,-5480,-11,17703,-19]}
|
||||
{"point":[-85,-16539,15059,-5483,-11,17699,-20]}
|
||||
{"point":[-85,-16541,15063,-5484,-11,17690,-21]}
|
||||
{"point":[-85,-16542,15069,-5486,-16,17663,-21]}
|
||||
{"point":[-86,-16541,15076,-5484,-19,17649,-21]}
|
||||
{"point":[-85,-16539,15080,-5486,-22,17633,-23]}
|
||||
{"point":[-85,-16541,15091,-5487,-31,17608,-21]}
|
||||
{"point":[-85,-16541,15096,-5487,-33,17564,-23]}
|
||||
{"point":[-86,-16542,15099,-5487,-37,17543,-23]}
|
||||
{"point":[-86,-16541,15109,-5491,-39,17523,-23]}
|
||||
{"point":[-85,-16542,15111,-5493,-38,17490,-23]}
|
||||
{"point":[-86,-16542,15114,-5495,-38,17473,-23]}
|
||||
{"point":[-86,-16544,15118,-5500,-41,17457,-23]}
|
||||
{"point":[-85,-16544,15118,-5502,-39,17438,-24]}
|
||||
{"point":[-85,-16545,15118,-5504,-38,17394,-21]}
|
||||
{"point":[-83,-16544,15118,-5511,-35,17368,-23]}
|
||||
{"point":[-86,-16544,15118,-5516,-35,17337,-21]}
|
||||
{"point":[-85,-16544,15120,-5520,-34,17284,-23]}
|
||||
{"point":[-85,-16544,15118,-5520,-33,17259,-23]}
|
||||
{"point":[-86,-16544,15117,-5523,-30,17233,-23]}
|
||||
{"point":[-86,-16544,15117,-5523,-15,17173,-23]}
|
||||
{"point":[-85,-16545,15115,-5523,-13,17135,-24]}
|
||||
{"point":[-85,-16544,15110,-5522,-12,17094,-21]}
|
||||
{"point":[-86,-16545,15103,-5522,-13,17056,-23]}
|
||||
{"point":[-85,-16545,15096,-5523,-16,16987,-24]}
|
||||
{"point":[-86,-16546,15087,-5524,-19,16954,-23]}
|
||||
{"point":[-85,-16548,15071,-5526,-23,16914,-23]}
|
||||
{"point":[-85,-16549,15060,-5526,-28,16846,-23]}
|
||||
{"point":[-86,-16549,15040,-5526,-31,16807,-23]}
|
||||
{"point":[-86,-16550,15028,-5528,-35,16770,-23]}
|
||||
{"point":[-86,-16550,15019,-5530,-39,16728,-23]}
|
||||
{"point":[-86,-16552,15011,-5530,-38,16636,-24]}
|
||||
{"point":[-85,-16555,14995,-5530,-41,16593,-23]}
|
||||
{"point":[-85,-16553,14988,-5534,-42,16556,-23]}
|
||||
{"point":[-86,-16555,14978,-5534,-43,16476,-24]}
|
||||
{"point":[-85,-16555,14953,-5537,-42,16435,-24]}
|
||||
{"point":[-86,-16553,14945,-5537,-43,16397,-23]}
|
||||
{"point":[-85,-16556,14929,-5539,-43,16316,-24]}
|
||||
{"point":[-86,-16555,14911,-5538,-46,16278,-24]}
|
||||
{"point":[-85,-16557,14870,-5541,-45,16232,-26]}
|
||||
{"point":[-85,-16559,14849,-5544,-45,16141,-23]}
|
||||
{"point":[-85,-16557,14824,-5542,-45,16090,-23]}
|
||||
{"point":[-85,-16560,14779,-5544,-46,16044,-24]}
|
||||
{"point":[-86,-16560,14761,-5544,-45,16004,-24]}
|
||||
{"point":[-86,-16561,14751,-5545,-45,15911,-23]}
|
||||
{"point":[-85,-16561,14731,-5545,-43,15862,-24]}
|
||||
{"point":[-86,-16564,14724,-5546,-45,15802,-23]}
|
||||
{"point":[-86,-16563,14723,-5549,-45,15695,-26]}
|
||||
{"point":[-86,-16567,14714,-5549,-45,15632,-24]}
|
||||
{"point":[-86,-16567,14712,-5548,-48,15558,-26]}
|
||||
{"point":[-86,-16568,14710,-5549,-43,15435,-24]}
|
||||
{"point":[-86,-16568,14709,-5548,-45,15358,-24]}
|
||||
{"point":[-85,-16572,14710,-5549,-46,15286,-26]}
|
||||
{"point":[-85,-16575,14710,-5550,-49,15126,-24]}
|
||||
{"point":[-86,-16578,14710,-5553,-48,15044,-24]}
|
||||
{"point":[-85,-16583,14710,-5556,-49,14960,-26]}
|
||||
{"point":[-87,-16585,14710,-5560,-48,14861,-26]}
|
||||
{"point":[-85,-16589,14712,-5564,-50,14702,-26]}
|
||||
{"point":[-86,-16593,14712,-5567,-50,14615,-26]}
|
||||
{"point":[-85,-16601,14710,-5575,-50,14507,-27]}
|
||||
{"point":[-83,-16603,14712,-5579,-50,14422,-27]}
|
||||
{"point":[-83,-16610,14712,-5587,-52,14224,-27]}
|
||||
{"point":[-85,-16614,14712,-5593,-53,14143,-26]}
|
||||
{"point":[-85,-16622,14710,-5612,-54,14041,-27]}
|
||||
{"point":[-85,-16625,14709,-5623,-57,13840,-27]}
|
||||
{"point":[-86,-16629,14710,-5637,-59,13730,-28]}
|
||||
{"point":[-86,-16636,14710,-5664,-59,13614,-28]}
|
||||
{"point":[-86,-16637,14710,-5679,-60,13476,-27]}
|
||||
{"point":[-87,-16641,14710,-5704,-59,13241,-26]}
|
||||
{"point":[-86,-16647,14709,-5754,-59,13094,-27]}
|
||||
{"point":[-85,-16648,14709,-5776,-63,12977,-28]}
|
||||
{"point":[-86,-16652,14710,-5799,-63,12696,-26]}
|
||||
{"point":[-85,-16656,14712,-5824,-63,12578,-28]}
|
||||
{"point":[-83,-16664,14712,-5880,-63,12436,-27]}
|
||||
{"point":[-82,-16673,14712,-5905,-65,12201,-27]}
|
||||
{"point":[-79,-16677,14710,-5936,-67,12076,-27]}
|
||||
{"point":[-76,-16695,14713,-5995,-67,11962,-27]}
|
||||
{"point":[-70,-16706,14712,-6024,-70,11846,-28]}
|
||||
{"point":[-64,-16717,14710,-6050,-65,11587,-28]}
|
||||
{"point":[-65,-16741,14710,-6113,-68,11458,-30]}
|
||||
{"point":[-64,-16756,14710,-6149,-71,11344,-28]}
|
||||
{"point":[-64,-16770,14712,-6183,-72,11061,-28]}
|
||||
{"point":[-61,-16783,14710,-6212,-72,10931,-28]}
|
||||
{"point":[-65,-16807,14712,-6274,-74,10811,-28]}
|
||||
{"point":[-65,-16820,14713,-6310,-71,10509,-28]}
|
||||
{"point":[-68,-16835,14713,-6344,-75,10372,-30]}
|
||||
{"point":[-64,-16865,14712,-6411,-75,10251,-28]}
|
||||
{"point":[-64,-16880,14710,-6454,-74,10092,-28]}
|
||||
{"point":[-64,-16898,14712,-6490,-72,9806,-28]}
|
||||
{"point":[-65,-16917,14712,-6534,-75,9684,-28]}
|
||||
{"point":[-59,-16958,14710,-6619,-74,9559,-30]}
|
||||
{"point":[-57,-16976,14709,-6660,-70,9283,-28]}
|
||||
{"point":[-60,-16993,14710,-6705,-71,9137,-28]}
|
||||
{"point":[-63,-17027,14710,-6803,-72,8985,-28]}
|
||||
{"point":[-61,-17048,14712,-6852,-72,8834,-28]}
|
||||
{"point":[-61,-17065,14710,-6906,-70,8544,-27]}
|
||||
{"point":[-63,-17086,14709,-6965,-72,8412,-28]}
|
||||
{"point":[-65,-17129,14709,-7087,-71,8243,-28]}
|
||||
{"point":[-60,-17152,14710,-7154,-76,8098,-28]}
|
||||
{"point":[-56,-17174,14710,-7224,-75,7845,-28]}
|
||||
{"point":[-59,-17222,14713,-7355,-72,7700,-27]}
|
||||
{"point":[-59,-17251,14712,-7425,-72,7454,-30]}
|
||||
{"point":[-60,-17280,14712,-7498,-72,7323,-28]}
|
||||
{"point":[-53,-17306,14712,-7570,-74,7196,-28]}
|
||||
{"point":[-46,-17366,14712,-7727,-74,7073,-28]}
|
||||
{"point":[-41,-17396,14712,-7807,-74,6796,-30]}
|
||||
{"point":[-33,-17425,14713,-7882,-74,6796,-28]}
|
||||
{"point":[-24,-17484,14712,-8042,-72,6514,-28]}
|
||||
{"point":[-27,-17516,14712,-8118,-74,6370,-28]}
|
||||
{"point":[-23,-17556,14713,-8191,-75,6076,-28]}
|
||||
{"point":[-20,-17624,14713,-8260,-76,5929,-30]}
|
||||
{"point":[-19,-17663,14712,-8410,-74,5802,-30]}
|
||||
{"point":[-20,-17699,14712,-8492,-74,5533,-30]}
|
||||
{"point":[-20,-17734,14713,-8581,-76,5399,-30]}
|
||||
{"point":[-15,-17811,14713,-8747,-79,5265,-28]}
|
||||
{"point":[-11,-17851,14714,-8828,-76,5144,-28]}
|
||||
{"point":[-12,-17892,14713,-8911,-72,4912,-28]}
|
||||
{"point":[-15,-17970,14712,-9085,-72,4753,-28]}
|
||||
{"point":[-15,-18014,14712,-9168,-75,4615,-28]}
|
||||
{"point":[-13,-18056,14710,-9245,-75,4356,-28]}
|
||||
{"point":[-15,-18139,14709,-9404,-72,4224,-28]}
|
||||
{"point":[-19,-18181,14710,-9497,-72,4093,-28]}
|
||||
{"point":[-19,-18219,14712,-9580,-78,3838,-28]}
|
||||
{"point":[-13,-18307,14714,-9688,-76,3696,-28]}
|
||||
{"point":[-13,-18355,14712,-9884,-76,3582,-30]}
|
||||
{"point":[-15,-18403,14713,-9976,-76,3477,-28]}
|
||||
{"point":[-16,-18452,14714,-10059,-76,3230,-30]}
|
||||
{"point":[-11,-18547,14716,-10259,-78,3118,-28]}
|
||||
{"point":[-6,-18597,14714,-10356,-76,3006,-30]}
|
||||
{"point":[-5,-18645,14716,-10445,-79,2808,-30]}
|
||||
{"point":[-6,-18696,14716,-10535,-74,2702,-28]}
|
||||
{"point":[-4,-18792,14713,-10732,-78,2579,-30]}
|
||||
{"point":[-1,-18842,14713,-10825,-78,2447,-30]}
|
||||
{"point":[-1,-18893,14714,-10924,-76,2231,-28]}
|
||||
{"point":[-1,-18994,14713,-11112,-76,2121,-27]}
|
||||
{"point":[1,-19046,14710,-11207,-71,2006,-30]}
|
||||
{"point":[-1,-19097,14712,-11307,-72,1772,-30]}
|
||||
{"point":[0,-19201,14713,-11515,-74,1685,-30]}
|
||||
{"point":[0,-19246,14713,-11624,-72,1555,-30]}
|
||||
{"point":[-4,-19293,14714,-11737,-78,1369,-28]}
|
||||
{"point":[-4,-19400,14714,-11843,-75,1263,-28]}
|
||||
{"point":[-5,-19452,14714,-12063,-74,1160,-30]}
|
||||
{"point":[-4,-19506,14714,-12175,-76,976,-30]}
|
||||
{"point":[1,-19620,14717,-12391,-76,885,-28]}
|
||||
{"point":[2,-19675,14716,-12503,-78,808,-30]}
|
||||
{"point":[5,-19731,14716,-12610,-76,638,-28]}
|
||||
{"point":[5,-19793,14717,-12718,-78,554,-28]}
|
||||
{"point":[5,-19910,14716,-12935,-75,472,-30]}
|
||||
{"point":[5,-19966,14716,-13031,-78,293,-28]}
|
||||
{"point":[5,-20026,14716,-13127,-74,210,-28]}
|
||||
{"point":[5,-20147,14716,-13320,-74,123,-28]}
|
||||
{"point":[4,-20208,14714,-13412,-74,42,-28]}
|
||||
{"point":[8,-20264,14714,-13506,-75,-114,-30]}
|
||||
{"point":[8,-20322,14714,-13610,-75,-184,-28]}
|
||||
{"point":[5,-20433,14714,-13871,-71,-255,-28]}
|
||||
{"point":[1,-20489,14716,-14011,-72,-414,-28]}
|
||||
{"point":[2,-20547,14714,-14125,-71,-498,-30]}
|
||||
{"point":[0,-20658,14713,-14338,-72,-565,-28]}
|
||||
{"point":[-1,-20720,14713,-14442,-71,-644,-31]}
|
||||
{"point":[0,-20779,14714,-14551,-70,-788,-28]}
|
||||
{"point":[1,-20898,14716,-14772,-68,-852,-30]}
|
||||
{"point":[5,-20961,14716,-14875,-68,-911,-30]}
|
||||
{"point":[5,-21021,14717,-14979,-68,-1047,-30]}
|
||||
{"point":[4,-21144,14717,-15091,-71,-1126,-28]}
|
||||
{"point":[5,-21209,14717,-15309,-70,-1182,-30]}
|
||||
{"point":[8,-21273,14719,-15419,-70,-1240,-28]}
|
||||
{"point":[5,-21335,14717,-15527,-68,-1354,-30]}
|
||||
{"point":[5,-21468,14714,-15751,-67,-1403,-27]}
|
||||
{"point":[5,-21531,14716,-15868,-67,-1487,-30]}
|
||||
{"point":[6,-21596,14717,-15989,-65,-1540,-30]}
|
||||
{"point":[5,-21659,14714,-16116,-67,-1582,-28]}
|
||||
{"point":[6,-21781,14719,-16351,-64,-1621,-28]}
|
||||
{"point":[6,-21846,14719,-16472,-70,-1694,-28]}
|
||||
{"point":[6,-21976,14716,-16588,-65,-1737,-30]}
|
||||
{"point":[5,-22044,14717,-16817,-67,-1775,-30]}
|
||||
{"point":[5,-22105,14719,-16934,-74,-1855,-28]}
|
||||
{"point":[5,-22239,14719,-17135,-74,-1899,-30]}
|
||||
{"point":[6,-22305,14719,-17252,-75,-1945,-28]}
|
||||
{"point":[6,-22372,14717,-17357,-75,-2031,-30]}
|
||||
{"point":[5,-22439,14716,-17462,-75,-2076,-30]}
|
||||
{"point":[6,-22576,14716,-17682,-75,-2112,-30]}
|
||||
{"point":[5,-22644,14716,-17791,-74,-2160,-28]}
|
||||
{"point":[5,-22712,14716,-17899,-70,-2239,-30]}
|
||||
{"point":[6,-22847,14717,-18135,-72,-2279,-28]}
|
||||
{"point":[5,-22916,14717,-18246,-72,-2315,-28]}
|
||||
{"point":[6,-22982,14716,-18347,-72,-2371,-28]}
|
||||
{"point":[6,-23049,14717,-18444,-76,-2410,-28]}
|
||||
{"point":[5,-23179,14717,-18674,-75,-2443,-30]}
|
||||
{"point":[6,-23247,14717,-18800,-75,-2503,-30]}
|
||||
{"point":[8,-23314,14717,-18923,-78,-2539,-30]}
|
||||
{"point":[6,-23462,14719,-19160,-78,-2568,-28]}
|
||||
{"point":[6,-23521,14720,-19278,-79,-2602,-30]}
|
||||
{"point":[6,-23590,14721,-19388,-78,-2675,-30]}
|
||||
{"point":[6,-23734,14720,-19606,-79,-2713,-30]}
|
||||
{"point":[8,-23810,14719,-19712,-78,-2746,-30]}
|
||||
{"point":[6,-23882,14721,-19819,-81,-2816,-28]}
|
||||
{"point":[6,-24020,14719,-20033,-78,-2856,-30]}
|
||||
{"point":[8,-24091,14719,-20139,-75,-2923,-30]}
|
||||
{"point":[6,-24153,14720,-20254,-76,-2964,-30]}
|
||||
{"point":[5,-24285,14719,-20367,-78,-2989,-30]}
|
||||
{"point":[6,-24351,14716,-20591,-74,-3013,-30]}
|
||||
{"point":[6,-24415,14714,-20705,-75,-3067,-28]}
|
||||
{"point":[6,-24550,14716,-20815,-75,-3088,-28]}
|
||||
{"point":[6,-24610,14717,-21034,-75,-3116,-28]}
|
||||
{"point":[6,-24673,14720,-21143,-79,-3179,-30]}
|
||||
{"point":[5,-24739,14720,-21248,-76,-3202,-28]}
|
||||
{"point":[5,-24873,14720,-21467,-79,-3228,-30]}
|
||||
{"point":[6,-24937,14719,-21564,-79,-3251,-30]}
|
||||
{"point":[6,-25013,14717,-21669,-78,-3301,-30]}
|
||||
{"point":[6,-25143,14720,-21884,-78,-3337,-30]}
|
||||
{"point":[5,-25223,14723,-21983,-76,-3360,-30]}
|
||||
{"point":[6,-25280,14728,-22085,-76,-3412,-28]}
|
||||
{"point":[9,-25348,14735,-22181,-76,-3431,-28]}
|
||||
{"point":[8,-25481,14730,-22377,-72,-3449,-28]}
|
||||
{"point":[9,-25547,14723,-22478,-74,-3479,-28]}
|
||||
{"point":[8,-25617,14720,-22572,-75,-3496,-28]}
|
||||
{"point":[8,-25741,14717,-22788,-75,-3511,-27]}
|
||||
{"point":[8,-25804,14719,-22909,-72,-3526,-30]}
|
||||
{"point":[6,-25861,14721,-23026,-74,-3551,-30]}
|
||||
{"point":[8,-25920,14719,-23135,-74,-3593,-30]}
|
||||
{"point":[4,-26033,14717,-23329,-74,-3614,-28]}
|
||||
{"point":[2,-26095,14717,-23421,-74,-3640,-30]}
|
||||
{"point":[0,-26148,14717,-23512,-72,-3648,-28]}
|
||||
{"point":[-2,-26268,14717,-23611,-72,-3651,-28]}
|
||||
{"point":[-2,-26328,14717,-23789,-70,-3654,-28]}
|
||||
{"point":[-1,-26385,14719,-23871,-68,-3654,-30]}
|
||||
{"point":[0,-26438,14717,-23962,-68,-3655,-28]}
|
||||
{"point":[1,-26555,14719,-24128,-65,-3655,-28]}
|
||||
{"point":[4,-26619,14719,-24212,-68,-3655,-28]}
|
||||
{"point":[4,-26666,14719,-24294,-63,-3657,-28]}
|
||||
{"point":[5,-26775,14716,-24451,-60,-3655,-28]}
|
||||
{"point":[5,-26828,14717,-24529,-61,-3657,-28]}
|
||||
{"point":[6,-26890,14717,-24603,-57,-3655,-28]}
|
||||
{"point":[6,-26986,14720,-24742,-59,-3657,-28]}
|
||||
{"point":[6,-27036,14717,-24812,-54,-3655,-28]}
|
||||
{"point":[6,-27082,14717,-24878,-54,-3657,-28]}
|
||||
{"point":[5,-27129,14717,-24949,-53,-3655,-28]}
|
||||
{"point":[0,-27218,14717,-25094,-56,-3655,-27]}
|
||||
{"point":[0,-27262,14717,-25157,-54,-3657,-30]}
|
||||
{"point":[1,-27305,14716,-25220,-54,-3655,-28]}
|
||||
{"point":[-5,-27383,14714,-25349,-52,-3654,-28]}
|
||||
{"point":[-5,-27426,14716,-25411,-52,-3655,-28]}
|
||||
{"point":[-9,-27463,14716,-25471,-53,-3655,-28]}
|
||||
{"point":[-9,-27494,14717,-25532,-53,-3657,-28]}
|
||||
{"point":[-15,-27566,14716,-25638,-54,-3657,-30]}
|
||||
{"point":[-20,-27592,14716,-25690,-53,-3655,-28]}
|
||||
{"point":[-28,-27619,14716,-25742,-53,-3654,-28]}
|
||||
{"point":[-31,-27670,14716,-25838,-50,-3657,-28]}
|
||||
{"point":[-31,-27693,14716,-25881,-52,-3657,-28]}
|
||||
{"point":[-34,-27714,14713,-25923,-49,-3654,-28]}
|
||||
{"point":[-45,-27752,14714,-25990,-49,-3654,-28]}
|
||||
{"point":[-56,-27773,14713,-26015,-48,-3655,-28]}
|
||||
{"point":[-60,-27788,14714,-26039,-49,-3654,-28]}
|
||||
{"point":[-71,-27810,14713,-26071,-46,-3654,-28]}
|
||||
{"point":[-78,-27822,14712,-26084,-42,-3653,-28]}
|
||||
{"point":[-85,-27828,14712,-26098,-46,-3653,-28]}
|
||||
{"point":[-89,-27839,14710,-26107,-46,-3651,-28]}
|
||||
{"point":[-89,-27842,14709,-26122,-43,-3651,-28]}
|
||||
{"point":[-90,-27844,14708,-26125,-45,-3647,-27]}
|
||||
{"point":[-90,-27844,14706,-26126,-43,-3646,-27]}
|
||||
{"point":[-92,-27847,14703,-26125,-42,-3643,-28]}
|
||||
{"point":[-93,-27844,14703,-26125,-41,-3633,-28]}
|
||||
{"point":[-92,-27843,14702,-26122,-41,-3628,-28]}
|
||||
{"point":[-93,-27840,14702,-26122,-39,-3621,-27]}
|
||||
{"point":[-93,-27842,14702,-26120,-35,-3607,-28]}
|
||||
{"point":[-93,-27839,14702,-26117,-26,-3574,-28]}
|
||||
{"point":[-90,-27840,14703,-26115,-20,-3548,-28]}
|
||||
{"point":[-87,-27838,14703,-26114,-9,-3511,-27]}
|
||||
{"point":[-89,-27838,14703,-26113,-9,-3412,-27]}
|
||||
{"point":[-90,-27839,14702,-26111,-6,-3352,-27]}
|
||||
{"point":[-90,-27838,14702,-26106,-6,-3295,-28]}
|
||||
{"point":[-93,-27838,14702,-26102,-5,-3175,-27]}
|
||||
{"point":[-90,-27839,14703,-26096,-6,-3103,-28]}
|
||||
{"point":[-92,-27838,14702,-26085,-8,-3033,-28]}
|
||||
{"point":[-92,-27836,14701,-26074,-5,-2958,-27]}
|
||||
{"point":[-92,-27838,14701,-26062,-8,-2789,-28]}
|
||||
{"point":[-93,-27835,14701,-26037,-6,-2697,-27]}
|
||||
{"point":[-92,-27835,14701,-26021,-8,-2500,-28]}
|
||||
{"point":[-93,-27835,14702,-25997,-6,-2400,-28]}
|
||||
{"point":[-92,-27836,14699,-25971,-8,-2308,-28]}
|
||||
{"point":[-92,-27839,14701,-25901,-8,-2208,-30]}
|
||||
{"point":[-92,-27836,14701,-25852,-6,-2007,-28]}
|
||||
{"point":[-87,-27839,14703,-25771,-8,-1897,-27]}
|
||||
{"point":[-86,-27839,14705,-25721,-8,-1771,-28]}
|
||||
{"point":[-85,-27839,14705,-25666,-9,-1507,-27]}
|
||||
{"point":[-86,-27838,14702,-25605,-8,-1355,-27]}
|
||||
{"point":[-86,-27838,14703,-25499,-8,-1196,-27]}
|
||||
{"point":[-87,-27836,14703,-25433,-16,-863,-27]}
|
||||
{"point":[-86,-27836,14701,-25355,-17,-701,-28]}
|
||||
{"point":[-87,-27835,14701,-25210,-19,-537,-28]}
|
||||
{"point":[-89,-27833,14701,-25129,-22,-366,-28]}
|
||||
{"point":[-90,-27833,14699,-25047,-27,-38,-27]}
|
||||
{"point":[-93,-27831,14701,-24852,-24,112,-28]}
|
||||
{"point":[-92,-27829,14699,-24754,-20,266,-28]}
|
||||
{"point":[-92,-27829,14701,-24656,-27,646,-27]}
|
||||
{"point":[-92,-27829,14699,-24548,-26,848,-27]}
|
||||
{"point":[-93,-27829,14699,-24329,-26,1060,-28]}
|
||||
{"point":[-92,-27829,14701,-24216,-28,1518,-28]}
|
||||
{"point":[-90,-27828,14699,-24082,-35,1737,-28]}
|
||||
{"point":[-92,-27828,14701,-23849,-34,1962,-28]}
|
||||
{"point":[-90,-27827,14697,-23714,-34,2167,-28]}
|
||||
{"point":[-90,-27827,14699,-23564,-37,2562,-27]}
|
||||
{"point":[-89,-27825,14701,-23440,-37,2819,-27]}
|
||||
{"point":[-82,-27827,14702,-23163,-39,3029,-28]}
|
||||
{"point":[-79,-27827,14703,-22995,-42,3507,-27]}
|
||||
{"point":[-78,-27827,14702,-22814,-38,3782,-28]}
|
||||
{"point":[-74,-27829,14701,-22473,-42,4022,-28]}
|
||||
{"point":[-78,-27825,14701,-22313,-37,4279,-27]}
|
||||
{"point":[-78,-27824,14698,-22119,-38,4754,-26]}
|
||||
{"point":[-78,-27810,14698,-21960,-42,4964,-27]}
|
||||
{"point":[-75,-27806,14706,-21643,-43,5222,-28]}
|
||||
{"point":[-74,-27802,14705,-21426,-45,5776,-27]}
|
||||
{"point":[-71,-27789,14703,-21294,-48,6053,-26]}
|
||||
{"point":[-74,-27784,14703,-20890,-48,6343,-26]}
|
||||
{"point":[-72,-27776,14703,-20716,-43,6762,-27]}
|
||||
{"point":[-75,-27770,14703,-20512,-48,6985,-24]}
|
||||
{"point":[-71,-27750,14703,-20120,-41,7245,-26]}
|
||||
{"point":[-67,-27735,14699,-19894,-38,7800,-26]}
|
||||
{"point":[-70,-27715,14699,-19725,-38,8040,-26]}
|
||||
{"point":[-70,-27685,14699,-19533,-39,8319,-26]}
|
||||
{"point":[-71,-27664,14702,-19132,-41,8576,-24]}
|
||||
{"point":[-75,-27638,14702,-18877,-41,9014,-26]}
|
||||
{"point":[-85,-27592,14705,-18450,-43,9251,-26]}
|
||||
{"point":[-86,-27568,14705,-18262,-38,9722,-26]}
|
||||
{"point":[-87,-27537,14701,-18038,-34,9935,-24]}
|
||||
{"point":[-92,-27496,14697,-17793,-27,10188,-26]}
|
||||
{"point":[-92,-27438,14699,-17413,-22,10443,-24]}
|
||||
{"point":[-93,-27393,14699,-17222,-24,10909,-24]}
|
||||
{"point":[-92,-27354,14697,-17020,-27,11137,-24]}
|
||||
{"point":[-89,-27261,14701,-16589,-28,11365,-24]}
|
||||
{"point":[-90,-27215,14701,-16358,-37,11579,-26]}
|
||||
{"point":[-90,-27165,14702,-16089,-30,11778,-24]}
|
||||
{"point":[-92,-27121,14701,-15867,-19,12157,-24]}
|
||||
{"point":[-90,-27022,14697,-15379,-6,12374,-24]}
|
||||
{"point":[-92,-26974,14697,-15187,-9,12814,-24]}
|
||||
{"point":[-92,-26913,14695,-14967,-8,13042,-24]}
|
||||
{"point":[-85,-26801,14697,-14724,-13,13281,-24]}
|
||||
{"point":[-81,-26738,14703,-14290,-24,13513,-24]}
|
||||
{"point":[-82,-26674,14702,-14069,-41,13881,-26]}
|
||||
{"point":[-82,-26545,14701,-13521,-42,14070,-26]}
|
||||
{"point":[-85,-26478,14698,-13312,-35,14239,-24]}
|
||||
{"point":[-87,-26411,14694,-13119,-35,14584,-24]}
|
||||
{"point":[-90,-26334,14695,-12897,-37,14762,-24]}
|
||||
{"point":[-90,-26185,14695,-12415,-35,14948,-24]}
|
||||
{"point":[-90,-26107,14697,-12209,-42,15290,-26]}
|
||||
{"point":[-89,-26032,14698,-11931,-43,15464,-24]}
|
||||
{"point":[-86,-25876,14697,-11413,-48,15630,-26]}
|
||||
{"point":[-86,-25809,14692,-11199,-37,15956,-23]}
|
||||
{"point":[-89,-25725,14684,-10956,-33,16114,-24]}
|
||||
{"point":[-93,-25555,14676,-10487,-35,16257,-24]}
|
||||
{"point":[-92,-25449,14673,-10239,-35,16567,-23]}
|
||||
{"point":[-92,-25353,14675,-10045,-41,16691,-23]}
|
||||
{"point":[-89,-25176,14670,-9532,-45,16803,-26]}
|
||||
{"point":[-87,-25066,14669,-9288,-53,16897,-24]}
|
||||
{"point":[-83,-24967,14664,-9018,-53,17159,-24]}
|
||||
{"point":[-83,-24867,14654,-8791,-52,17303,-26]}
|
||||
{"point":[-90,-24661,14633,-8279,-48,17420,-26]}
|
||||
{"point":[-92,-24570,14624,-8054,-52,17674,-26]}
|
||||
{"point":[-85,-24456,14611,-7796,-53,17811,-24]}
|
||||
{"point":[-82,-24242,14607,-7575,-57,17895,-24]}
|
||||
{"point":[-79,-24132,14576,-7021,-64,17981,-26]}
|
||||
{"point":[-74,-24027,14559,-6758,-64,18179,-26]}
|
||||
{"point":[-74,-23796,14532,-6295,-60,18275,-26]}
|
||||
{"point":[-79,-23678,14522,-6067,-61,18354,-26]}
|
||||
{"point":[-87,-23553,14510,-5876,-64,18513,-27]}
|
||||
{"point":[-89,-23431,14499,-5619,-67,18579,-24]}
|
||||
{"point":[-89,-23181,14481,-5108,-65,18643,-26]}
|
||||
{"point":[-83,-23054,14475,-4877,-64,18689,-26]}
|
||||
{"point":[-78,-22934,14459,-4600,-64,18804,-24]}
|
||||
{"point":[-76,-22679,14433,-4080,-63,18853,-26]}
|
||||
{"point":[-78,-22550,14419,-3853,-68,18893,-26]}
|
||||
{"point":[-82,-22413,14400,-3628,-65,18996,-26]}
|
||||
{"point":[-86,-22270,14386,-3407,-65,19037,-24]}
|
||||
{"point":[-86,-22016,14348,-2922,-67,19065,-26]}
|
||||
{"point":[-83,-21876,14331,-2717,-65,19149,-26]}
|
||||
{"point":[-79,-21737,14316,-2434,-71,19147,-26]}
|
||||
{"point":[-79,-21457,14278,-1980,-67,19189,-27]}
|
||||
{"point":[-79,-21312,14268,-1783,-64,19202,-26]}
|
||||
{"point":[-85,-21163,14250,-1532,-68,19213,-24]}
|
||||
{"point":[-86,-20875,14224,-1102,-61,19224,-26]}
|
||||
{"point":[-81,-20732,14212,-898,-65,19220,-24]}
|
||||
{"point":[-81,-20584,14195,-653,-61,19223,-23]}
|
||||
{"point":[-82,-20435,14183,-435,-64,19215,-24]}
|
||||
{"point":[-75,-20154,14151,79,-59,19217,-26]}
|
||||
{"point":[-75,-19995,14139,292,-52,19227,-23]}
|
||||
{"point":[-81,-19837,14135,479,-50,19206,-24]}
|
||||
{"point":[-86,-19526,14092,922,-48,19208,-24]}
|
||||
{"point":[-90,-19379,14081,1093,-46,19205,-24]}
|
||||
{"point":[-92,-19197,14076,1325,-48,19193,-24]}
|
||||
{"point":[-82,-18896,14044,1735,-46,19194,-24]}
|
||||
{"point":[-83,-18737,14028,1995,-48,19161,-24]}
|
||||
{"point":[-86,-18576,14014,2182,-39,19162,-24]}
|
||||
{"point":[-89,-18255,13991,2428,-31,19164,-26]}
|
||||
{"point":[-92,-18090,13966,2820,-30,19169,-26]}
|
||||
{"point":[-90,-17926,13962,3019,-5,19091,-24]}
|
||||
{"point":[-92,-17589,13923,3378,-2,19094,-26]}
|
||||
{"point":[-90,-17421,13908,3610,-4,19065,-24]}
|
||||
{"point":[-90,-17256,13901,3786,-2,19047,-24]}
|
||||
{"point":[-86,-17093,13885,4025,-5,19031,-26]}
|
||||
{"point":[-87,-16759,13852,4379,-8,19011,-26]}
|
||||
{"point":[-90,-16596,13823,4578,-5,18995,-24]}
|
||||
{"point":[-89,-16428,13801,4755,-6,18999,-24]}
|
||||
{"point":[-90,-16085,13775,5105,-5,18988,-24]}
|
||||
{"point":[-89,-15906,13750,5259,2,18925,-24]}
|
||||
{"point":[-90,-15722,13726,5442,-16,18869,-23]}
|
||||
{"point":[-89,-15556,13711,5623,-17,18833,-24]}
|
||||
{"point":[-81,-15222,13675,5980,-15,18823,-26]}
|
||||
{"point":[-82,-15044,13661,6151,-17,18801,-26]}
|
||||
{"point":[-85,-14709,13645,6337,-15,18774,-24]}
|
||||
{"point":[-89,-14523,13612,6645,-13,18794,-24]}
|
||||
{"point":[-86,-14345,13603,6819,-15,18808,-26]}
|
||||
{"point":[-83,-14169,13579,6992,0,18686,-26]}
|
||||
{"point":[-83,-13800,13537,7310,-5,18660,-24]}
|
||||
{"point":[-78,-13625,13517,7472,-4,18642,-24]}
|
||||
{"point":[-71,-13441,13499,7603,-6,18650,-24]}
|
||||
{"point":[-81,-13094,13485,7774,-13,18634,-26]}
|
||||
{"point":[-86,-12919,13448,8101,-24,18594,-26]}
|
||||
{"point":[-89,-12730,13432,8234,-39,18564,-24]}
|
||||
{"point":[-90,-12356,13418,8491,-38,18518,-26]}
|
||||
{"point":[-90,-12170,13411,8614,-35,18499,-26]}
|
||||
{"point":[-90,-11986,13402,8774,-34,18444,-26]}
|
||||
{"point":[-86,-11797,13384,8929,-31,18435,-24]}
|
||||
{"point":[-87,-11420,13351,9159,-34,18425,-24]}
|
||||
{"point":[-92,-11232,13327,9280,-33,18407,-26]}
|
||||
{"point":[-92,-11050,13310,9409,-42,18378,-26]}
|
||||
{"point":[-92,-10677,13274,9650,-41,18374,-26]}
|
||||
{"point":[-92,-10491,13260,9788,-42,18363,-27]}
|
||||
{"point":[-93,-10316,13245,9908,-39,18354,-26]}
|
||||
{"point":[-90,-10133,13233,10059,-42,18338,-27]}
|
||||
{"point":[-89,-9758,13198,10307,-41,18337,-26]}
|
||||
{"point":[-89,-9573,13176,10430,-43,18325,-26]}
|
||||
{"point":[-89,-9386,13160,10544,-50,18323,-26]}
|
||||
{"point":[-90,-8996,13126,10765,-46,18321,-26]}
|
||||
{"point":[-90,-8798,13108,10855,-49,18317,-27]}
|
||||
{"point":[-90,-8609,13087,10965,-42,18315,-26]}
|
||||
{"point":[-90,-8210,13046,11167,-37,18329,-27]}
|
||||
{"point":[-90,-8007,13025,11258,-43,18322,-26]}
|
||||
{"point":[-93,-7809,13001,11354,-39,18319,-27]}
|
||||
{"point":[-93,-7621,12983,11443,-42,18323,-27]}
|
||||
{"point":[-94,-7242,12952,11642,-38,18327,-26]}
|
||||
{"point":[-93,-7045,12936,11741,-34,18343,-24]}
|
||||
{"point":[-96,-6854,12921,11841,-37,18347,-26]}
|
||||
{"point":[-93,-6455,12904,12025,-38,18351,-26]}
|
||||
{"point":[-92,-6247,12888,12119,-42,18338,-27]}
|
||||
{"point":[-92,-6048,12874,12223,-34,18333,-26]}
|
||||
{"point":[-86,-5664,12843,12418,-28,18343,-26]}
|
||||
{"point":[-81,-5454,12834,12503,-33,18337,-27]}
|
||||
{"point":[-82,-5251,12811,12594,-30,18340,-26]}
|
||||
{"point":[-83,-5051,12789,12683,-33,18344,-24]}
|
||||
{"point":[-87,-4647,12752,12854,-30,18348,-26]}
|
||||
{"point":[-86,-4437,12741,12933,-33,18344,-26]}
|
||||
{"point":[-87,-4033,12727,13016,-30,18344,-24]}
|
||||
{"point":[-87,-3839,12701,13179,-26,18351,-26]}
|
||||
{"point":[-85,-3635,12682,13241,-13,18354,-26]}
|
||||
{"point":[-85,-3438,12659,13300,-9,18358,-26]}
|
||||
{"point":[-87,-3029,12612,13440,-17,18358,-26]}
|
||||
{"point":[-86,-2808,12594,13521,-35,18355,-26]}
|
||||
{"point":[-86,-2595,12582,13605,-34,18354,-27]}
|
||||
{"point":[-81,-2194,12573,13774,-33,18352,-27]}
|
||||
{"point":[-67,-1994,12569,13851,-30,18354,-26]}
|
||||
{"point":[-53,-1789,12556,13930,-35,18344,-27]}
|
||||
{"point":[-54,-1380,12535,14105,-34,18352,-26]}
|
||||
{"point":[-72,-1170,12525,14177,-35,18354,-26]}
|
||||
{"point":[-85,-947,12514,14245,-49,18347,-27]}
|
||||
{"point":[-85,-736,12490,14308,-50,18344,-27]}
|
||||
{"point":[-92,-331,12472,14447,-52,18349,-27]}
|
||||
{"point":[-93,-129,12468,14517,-48,18349,-27]}
|
||||
{"point":[-92,75,12459,14587,-46,18352,-27]}
|
||||
{"point":[-97,486,12437,14720,-49,18356,-27]}
|
||||
{"point":[-96,692,12431,14783,-52,18356,-27]}
|
||||
{"point":[-93,903,12424,14848,-56,18349,-27]}
|
||||
{"point":[-93,1308,12396,14977,-53,18349,-27]}
|
||||
{"point":[-93,1507,12381,15032,-48,18355,-26]}
|
||||
{"point":[-94,1707,12365,15087,-52,18348,-26]}
|
||||
{"point":[-96,1903,12344,15143,-52,18354,-26]}
|
||||
{"point":[-96,2308,12307,15250,-50,18355,-27]}
|
||||
{"point":[-90,2511,12295,15302,-50,18355,-27]}
|
||||
{"point":[-92,2719,12281,15350,-54,18349,-26]}
|
||||
{"point":[-81,3117,12249,15448,-52,18348,-27]}
|
||||
{"point":[-76,3312,12238,15488,-46,18354,-27]}
|
||||
{"point":[-72,3506,12231,15523,-50,18351,-28]}
|
||||
{"point":[-72,3701,12219,15560,-43,18355,-27]}
|
||||
{"point":[-71,4103,12150,15640,-46,18356,-28]}
|
||||
{"point":[-64,4302,12109,15676,-52,18355,-28]}
|
||||
{"point":[-74,4704,12089,15702,-54,18352,-28]}
|
||||
{"point":[-76,4891,12053,15766,-54,18348,-28]}
|
||||
{"point":[-74,5079,12038,15794,-53,18349,-27]}
|
||||
{"point":[-74,5266,12025,15814,-48,18351,-27]}
|
||||
{"point":[-93,5640,11980,15861,-46,18356,-27]}
|
||||
{"point":[-97,5837,11964,15873,-53,18356,-27]}
|
||||
{"point":[-97,6027,11947,15887,-52,18359,-27]}
|
||||
{"point":[-96,6411,11918,15902,-57,18354,-27]}
|
||||
{"point":[-94,6594,11896,15913,-50,18354,-27]}
|
||||
{"point":[-93,6770,11877,15921,-50,18354,-27]}
|
||||
{"point":[-94,7124,11857,15923,-54,18347,-28]}
|
||||
{"point":[-96,7293,11804,15943,-52,18354,-27]}
|
||||
{"point":[-96,7470,11781,15950,-50,18354,-27]}
|
||||
{"point":[-94,7654,11760,15957,-50,18352,-27]}
|
||||
{"point":[-87,8036,11708,15967,-50,18354,-27]}
|
||||
{"point":[-87,8190,11682,15972,-56,18341,-27]}
|
||||
{"point":[-86,8359,11656,15981,-52,18347,-27]}
|
||||
{"point":[-85,8690,11601,15987,-52,18347,-27]}
|
||||
{"point":[-89,8867,11578,15990,-53,18344,-27]}
|
||||
{"point":[-94,9004,11546,16000,-46,18356,-27]}
|
||||
{"point":[-97,9332,11501,16005,-49,18354,-27]}
|
||||
{"point":[-97,9515,11482,16014,-52,18354,-27]}
|
||||
{"point":[-94,9659,11464,16018,-53,18354,-27]}
|
||||
{"point":[-83,9852,11447,16022,-52,18348,-27]}
|
||||
{"point":[-81,10145,11416,16023,-48,18348,-28]}
|
||||
{"point":[-82,10316,11395,16022,-52,18347,-27]}
|
||||
{"point":[-85,10601,11355,16024,-49,18349,-28]}
|
||||
{"point":[-86,10769,11324,16029,-45,18351,-26]}
|
||||
{"point":[-83,10899,11288,16031,-48,18355,-27]}
|
||||
{"point":[-85,11204,11233,16030,-49,18354,-26]}
|
||||
{"point":[-83,11351,11206,16030,-52,18354,-28]}
|
||||
{"point":[-71,11504,11180,16030,-53,18352,-27]}
|
||||
{"point":[-38,11802,11144,16023,-54,18345,-27]}
|
||||
{"point":[-34,11955,11131,16027,-49,18347,-27]}
|
||||
{"point":[-37,12071,11126,16027,-48,18349,-27]}
|
||||
{"point":[-43,12208,11122,16027,-43,18351,-28]}
|
||||
{"point":[-60,12488,11049,16035,-43,18354,-27]}
|
||||
{"point":[-67,12602,11030,16035,-49,18356,-26]}
|
||||
{"point":[-78,12730,11016,16037,-49,18355,-28]}
|
||||
{"point":[-74,12999,10976,16034,-50,18355,-27]}
|
||||
{"point":[-60,13125,10958,16031,-52,18354,-27]}
|
||||
{"point":[-41,13249,10942,16034,-52,18354,-27]}
|
||||
{"point":[-33,13509,10927,16027,-54,18347,-27]}
|
||||
{"point":[-37,13642,10921,16020,-53,18345,-27]}
|
||||
{"point":[-42,13742,10904,16022,-52,18347,-28]}
|
||||
{"point":[-46,13962,10840,16023,-50,18348,-27]}
|
||||
{"point":[-43,14068,10780,16026,-46,18352,-27]}
|
||||
{"point":[-43,14177,10739,16024,-52,18352,-27]}
|
||||
{"point":[-50,14282,10713,16022,-50,18352,-28]}
|
||||
{"point":[-43,14488,10666,16024,-50,18352,-27]}
|
||||
{"point":[-34,14589,10647,16024,-52,18351,-28]}
|
||||
{"point":[-28,14705,10632,16026,-49,18351,-28]}
|
||||
{"point":[-30,14886,10606,16024,-50,18349,-28]}
|
||||
{"point":[-37,14989,10589,16022,-49,18351,-27]}
|
||||
{"point":[-39,15076,10566,16020,-54,18345,-27]}
|
||||
{"point":[-39,15174,10530,16015,-54,18344,-27]}
|
||||
{"point":[-54,15352,10465,16016,-53,18347,-27]}
|
||||
{"point":[-64,15431,10439,16016,-52,18347,-28]}
|
||||
{"point":[-71,15511,10413,16018,-52,18347,-27]}
|
||||
{"point":[-81,15667,10342,16018,-49,18348,-27]}
|
||||
{"point":[-81,15735,10314,16020,-49,18349,-27]}
|
||||
{"point":[-85,15795,10283,16022,-50,18351,-27]}
|
||||
{"point":[-90,15934,10226,16020,-52,18349,-27]}
|
||||
{"point":[-94,15998,10202,16020,-52,18349,-28]}
|
||||
{"point":[-97,16059,10174,16018,-53,18349,-28]}
|
||||
{"point":[-98,16119,10150,16018,-54,18349,-28]}
|
||||
{"point":[-98,16243,10103,16018,-54,18348,-28]}
|
||||
{"point":[-98,16288,10075,16019,-56,18349,-28]}
|
||||
{"point":[-98,16336,10047,16018,-54,18349,-26]}
|
||||
{"point":[-96,16436,9983,16018,-54,18348,-28]}
|
||||
{"point":[-96,16483,9953,16018,-53,18348,-28]}
|
||||
{"point":[-94,16535,9923,16018,-52,18348,-27]}
|
||||
{"point":[-87,16607,9886,16019,-53,18348,-28]}
|
||||
{"point":[-83,16645,9819,16019,-52,18348,-27]}
|
||||
{"point":[-78,16678,9784,16019,-49,18348,-27]}
|
||||
{"point":[-76,16708,9757,16020,-50,18349,-28]}
|
||||
{"point":[-75,16758,9710,16020,-48,18349,-28]}
|
||||
{"point":[-75,16780,9687,16022,-49,18352,-27]}
|
||||
{"point":[-76,16807,9663,16022,-48,18351,-27]}
|
||||
{"point":[-71,16828,9643,16022,-49,18349,-27]}
|
||||
{"point":[-67,16866,9598,16019,-48,18349,-27]}
|
||||
{"point":[-61,16883,9577,16016,-48,18347,-27]}
|
||||
{"point":[-60,16912,9547,16018,-46,18347,-27]}
|
||||
{"point":[-56,16921,9521,16016,-43,18347,-28]}
|
||||
{"point":[-48,16927,9482,16018,-33,18348,-28]}
|
||||
{"point":[-39,16931,9431,16020,-37,18349,-27]}
|
||||
{"point":[-39,16935,9393,16018,-37,18349,-27]}
|
||||
{"point":[-37,16936,9312,16020,-38,18351,-27]}
|
||||
{"point":[-34,16936,9286,16020,-45,18349,-27]}
|
||||
{"point":[-28,16935,9272,16020,-42,18349,-27]}
|
||||
{"point":[-26,16935,9262,16020,-41,18349,-27]}
|
||||
{"point":[-22,16932,9251,16019,-37,18349,-27]}
|
||||
{"point":[-20,16935,9232,16019,-19,18348,-27]}
|
||||
{"point":[-15,16931,9177,16019,-13,18347,-27]}
|
||||
{"point":[-15,16927,9137,16018,-16,18348,-26]}
|
||||
{"point":[-12,16925,9103,16018,-22,18348,-28]}
|
||||
26
HiveCoreR1/src/rm_arm_control/include/rm_arm_handler.hpp
Normal file
26
HiveCoreR1/src/rm_arm_control/include/rm_arm_handler.hpp
Normal file
@@ -0,0 +1,26 @@
|
||||
#include <cstdlib>
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/time.h>
|
||||
#include "rm_service.h"
|
||||
|
||||
#define ARM_DOF 6
|
||||
|
||||
class RmArmHandler
|
||||
{
|
||||
private:
|
||||
RM_Service roboticService_;
|
||||
rm_robot_handle robotHandle_;
|
||||
|
||||
public:
|
||||
RmArmHandler(RM_Service* roboticService, rm_robot_handle* robotHandle);
|
||||
~RmArmHandler();
|
||||
|
||||
void CustomApiLog(const char* message, va_list args);
|
||||
void ExecuteMovejCanfd(float points[ARM_DOF]);
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
45
HiveCoreR1/src/rm_arm_control/include/rm_arm_node.hpp
Normal file
45
HiveCoreR1/src/rm_arm_control/include/rm_arm_node.hpp
Normal file
@@ -0,0 +1,45 @@
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "std_msgs/msg/float64_multi_array.hpp"
|
||||
#include "sensor_msgs/msg/joint_state.hpp"
|
||||
#include "rm_service.h"
|
||||
#include "rm_arm_handler.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
using std::placeholders::_1;
|
||||
|
||||
|
||||
class RmArmNode: public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
RmArmNode();
|
||||
~RmArmNode();
|
||||
|
||||
|
||||
private:
|
||||
RM_Service *rmService_;
|
||||
rm_robot_handle *leftRmRobotHandler_;
|
||||
rm_robot_handle *rightRmRobotHandler_;
|
||||
|
||||
RmArmHandler *leftArmHandler_;
|
||||
RmArmHandler *rightArmHandler_;
|
||||
|
||||
static float left_q_in_joint[ARM_DOF];
|
||||
static float right_q_in_joint[ARM_DOF];
|
||||
|
||||
|
||||
rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr leftArmCommandSub_;
|
||||
rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr rightArmCommandSub_;
|
||||
|
||||
static rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr leftArmStatesPub_;
|
||||
static rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr rightArmStatesPub_;
|
||||
|
||||
static void CallbackRmRealtimeArmJointState(rm_realtime_arm_joint_state_t data);
|
||||
static void custom_api_log(const char* message, va_list args);
|
||||
|
||||
|
||||
void leftArmCommandCallback(const std_msgs::msg::Float64MultiArray::SharedPtr msg);
|
||||
void rightArmCommandCallback(const std_msgs::msg::Float64MultiArray::SharedPtr msg);
|
||||
|
||||
};
|
||||
|
||||
@@ -0,0 +1,23 @@
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from launch.actions import TimerAction
|
||||
|
||||
def generate_launch_description():
|
||||
# 创建节点启动描述
|
||||
rm_arm_control_node = Node(
|
||||
package='rm_arm_control', # 功能包名称
|
||||
executable='rm_arm_control', # 可执行文件名称
|
||||
name='rm_arm_control_node', # 节点名称（可自定义）
|
||||
output='screen', # 输出到屏幕
|
||||
parameters=[{"use_sim_time": False}] # 启用模拟时间
|
||||
)
|
||||
|
||||
start_rm_arm_control_node = TimerAction(
|
||||
period=1.0,
|
||||
actions=[rm_arm_control_node],
|
||||
)
|
||||
|
||||
# 组装launch描述
|
||||
return LaunchDescription([
|
||||
start_rm_arm_control_node
|
||||
])
|
||||
18
HiveCoreR1/src/rm_arm_control/package.xml
Normal file
18
HiveCoreR1/src/rm_arm_control/package.xml
Normal file
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>rm_arm_control</name>
|
||||
<version>1.0.0</version>
|
||||
<description>rm_arm_control package</description>
|
||||
<maintainer email="Ray@email.com">Your Name</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<build_depend>ament_cmake</build_depend>
|
||||
<exec_depend>rclcpp</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>nav_msgs</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
258
HiveCoreR1/src/rm_arm_control/readme.md
Normal file
258
HiveCoreR1/src/rm_arm_control/readme.md
Normal file
@@ -0,0 +1,258 @@
|
||||
# RMDemo_MovejCANFD
|
||||
|
||||
## **1. 项目介绍**
|
||||
本项目是一个使用睿尔曼C开发包,完成工程完成读取demo下的关节角度轨迹文件，按照10ms的周期进行透传，机械臂可以稳定运行，不同型号机械臂的透传轨迹文件，注册机械臂实时状态的回调函数，透传过程中，可以实时获取机械臂当前角度。
|
||||
|
||||
## **2. 代码结构**
|
||||
```
|
||||
RMDemo_MovejCANFD
|
||||
├── build # CMake构建生成的输出目录（如Makefile、构建文件等）
|
||||
├── cmake # CMake模块和脚本的存放目录
|
||||
│ ├── ...
|
||||
├── data
|
||||
│ └── robot_log.txt # 日志、轨迹文件等数据文件目录（在执行过程中生成）
|
||||
├── include # 自定义头文件存放目录
|
||||
├── Robotic_Arm 睿尔曼机械臂二次开发包
|
||||
│ ├── include
|
||||
│ │ ├── rm_define.h # 机械臂的定义
|
||||
│ │ └── rm_interface.h # 机械臂 API 的接口头文件
|
||||
│ ├── lib
|
||||
│ │ ├── api_c.dll # Windows 的 API 库
|
||||
│ │ ├── api_c.lib # Windows 的 API 库
|
||||
│ │ └── libapi_c.so # Linux 的 API 库
|
||||
├── src
|
||||
│ ├── main.c # 主函数
|
||||
├── CMakeLists.txt # 项目的顶层CMake配置文件
|
||||
├── readme.md # 为示例工程提供详细的文档
|
||||
├── run.bat # 快速运行脚本， Windows为bat脚本
|
||||
└── run.sh # 快速运行脚本， linux为shell脚本
|
||||
|
||||
```
|
||||
|
||||
## **3. 系统要求**
|
||||
|
||||
- 操作系统：Ubuntu 18.04或更高版本
|
||||
- 编译器：GCC 7.5或更高版本 (或任何其他兼容的C编译器)
|
||||
- 依赖库：
|
||||
- CMake 3.10或更高版本
|
||||
- RMAPI库(包含在 `Robotic_Arm/lib`目录中)
|
||||
|
||||
|
||||
## **4. 安装说明**
|
||||
|
||||
1. 克隆项目到本地：
|
||||
|
||||
```bash
|
||||
|
||||
```
|
||||
|
||||
2. 构建项目：
|
||||
Linux下：
|
||||
cmake:
|
||||
```bash
|
||||
mkdir build
|
||||
cd build
|
||||
cmake ..
|
||||
make
|
||||
|
||||
```
|
||||
|
||||
如果是GC编译的话 ：
|
||||
```bash
|
||||
#!/bin/bash
|
||||
# 编译并链接
|
||||
gcc -I./include -I./Robotic_Arm/include -L./Robotic_Arm/lib -Wl,-rpath=./Robotic_Arm/lib -DDATA_FILE_PATH=\"$(pwd)/data/RM65\&RM63_canfd_data.txt\" -o RMDemo_MovejCANFD src/main.c -lapi_c
|
||||
|
||||
# 检查编译是否成功
|
||||
if [ $? -eq 0 ]; then
|
||||
# 设置LD_LIBRARY_PATH环境变量
|
||||
export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:./Robotic_Arm/lib
|
||||
|
||||
# 运行编译后的可执行文件
|
||||
./RMDemo_MovejCANFD
|
||||
else
|
||||
echo "编译失败"
|
||||
fi
|
||||
```
|
||||
|
||||
|
||||
## **5. 注意事项**
|
||||
|
||||
该Demo以RM65-B型号机械臂为例，请根据实际情况修改代码中的数据。
|
||||
|
||||
## **6. 使用指南**
|
||||
|
||||
### **1. 快速运行**
|
||||
|
||||
按照以下步骤快速运行代码：
|
||||
|
||||
1. **配置机械臂IP地址**：打开 `main.c` 文件，在 `main` 函数中修改 `robot_ip_address` 类的初始化参数为当前机械臂的IP地址，默认IP地址为 `"192.168.1.18"`。
|
||||
|
||||
```C
|
||||
const char *robot_ip_address = "192.168.1.18";
|
||||
int robot_port = 8080;
|
||||
rm_robot_handle *robot_handle = rm_create_robot_arm(robot_ip_address, robot_port);
|
||||
```
|
||||
2. **命令行运行**：在终端进入 `RMDemo_MovejCANFD` 目录，输入以下命令运行 C程序：
|
||||
|
||||
2.1 Linux下
|
||||
* ```bash
|
||||
chmod +x run.sh
|
||||
./run.sh
|
||||
```
|
||||
|
||||
2.2 Windows下: 双击运行 run.bat
|
||||
|
||||
2.3 **选择对应型号轨迹文件**：
|
||||
- 在方法中`demo_movej_canfd`中：
|
||||
```C
|
||||
void demo_movej_canfd(rm_robot_handle* handle) {
|
||||
printf("Trying to open file: %s\n", DATA_FILE_PATH);
|
||||
|
||||
FILE* file = fopen(DATA_FILE_PATH, "r");
|
||||
if (!file) {
|
||||
perror("Failed to open file");
|
||||
return;
|
||||
}
|
||||
|
||||
float points[MAX_POINTS][ARM_DOF];
|
||||
int point_count = 0;
|
||||
while (fscanf(file, "%f,%f,%f,%f,%f,%f,%f",
|
||||
&points[point_count][0], &points[point_count][1], &points[point_count][2],
|
||||
&points[point_count][3], &points[point_count][4], &points[point_count][5],
|
||||
&points[point_count][6]) == ARM_DOF) {
|
||||
point_count++;
|
||||
}
|
||||
|
||||
```
|
||||
- ECO65：ECO65_canfd_data.txt
|
||||
- RM65&RML63-Ⅱ：RM65&RM63_canfd_data.txt
|
||||
- RM75：RM75_canfd_data.txt
|
||||
- DATA_FILE_PATH 为了跨平台在[CMakeLists.txt](CMakeLists.txt) 里面通过 # 将数据文件路径定义为预处理器宏
|
||||
add_definitions(-DDATA_FILE_PATH="${CMAKE_CURRENT_SOURCE_DIR}/data/RM65&RM63_canfd_data.txt")
|
||||
-
|
||||
|
||||
|
||||
### **2. 代码说明**
|
||||
|
||||
下面是 `main.c` 文件的主要功能：
|
||||
|
||||
- **连接机械臂**
|
||||
```C
|
||||
rm_robot_handle *robot_handle = rm_create_robot_arm(robot_ip_address, robot_port);
|
||||
```
|
||||
连接到指定IP和端口的机械臂。
|
||||
|
||||
- **获取API版本**
|
||||
|
||||
```C
|
||||
char *api_version = rm_api_version();
|
||||
printf("API Version: %s.\n", api_version);
|
||||
```
|
||||
获取并显示API版本。
|
||||
|
||||
|
||||
- **配置实时推送**
|
||||
|
||||
```C
|
||||
rm_realtime_push_config_t config = {5, true, 8089, 0, "192.168.1.88"};
|
||||
int result = rm_set_realtime_push(robot_handle, config);
|
||||
```
|
||||
|
||||
- **读取关节角度轨迹文件并进行透传**
|
||||
|
||||
```C
|
||||
demo_movej_canfd(robot_handle)
|
||||
```
|
||||
|
||||
|
||||
- **断开机械臂连接**
|
||||
|
||||
```C
|
||||
disconnect_robot_arm(robot_handle);
|
||||
```
|
||||
|
||||
### **3. 运行结果示例**
|
||||
|
||||
运行脚本后，输出结果如下所示：
|
||||
|
||||
```
|
||||
API Version: 0.3.0.
|
||||
Robot handle created successfully: 1
|
||||
Joint positions:
|
||||
0.01 -21.32 -78.51 -0.03 -80.17 0.02
|
||||
Joint positions:
|
||||
0.01 -21.32 -78.51 -0.03 -80.17 0.02
|
||||
Joint positions:
|
||||
0.01 -21.32 -78.51 -0.03 -80.17 0.02
|
||||
Joint positions:
|
||||
0.01 -21.32 -78.51 -0.03 -80.17 0.02
|
||||
Joint positions:
|
||||
0.01 -21.32 -78.51 -0.03 -80.17 0.02
|
||||
Joint positions:
|
||||
0.01 -21.32 -78.51 -0.03 -80.17 0.02
|
||||
Joint positions:
|
||||
0.01 -21.32 -78.51 -0.03 -80.17 0.02
|
||||
Successfully set realtime push configuration.
|
||||
Current state: -53.000
|
||||
Current angles: 0.013 -21.323 -78.511 -0.034 -80.170 0.020
|
||||
Current state: 0
|
||||
Current state: 0
|
||||
Error Code: 0
|
||||
Arm IP: 192.168.1.18
|
||||
Arm Error: 0
|
||||
Joint Position:
|
||||
0.013
|
||||
-21.323
|
||||
-78.511
|
||||
-0.034
|
||||
-80.170
|
||||
0.020
|
||||
Force Sensor:
|
||||
Coordinate: 888729056
|
||||
System Error: 0
|
||||
Waypoint:
|
||||
Euler: [3.141, 0.000, 0.000]
|
||||
Position: [0.300, -0.000, 0.299]
|
||||
Quat: [0.000, 1.000, -0.000, -0.000]
|
||||
....
|
||||
176.12 -128.64 133.60 176.12 126.63 356.12
|
||||
Pass-through completed
|
||||
|
||||
The motion is complete, the arm is in place.
|
||||
Motion result: 1
|
||||
Current device: 0
|
||||
Is the next trajectory connected: 0
|
||||
movej_cmd joint movement 1: 0
|
||||
...
|
||||
Joint positions:
|
||||
-0.00 0.00 0.00 0.00 0.00 -0.00
|
||||
INFO: disconnect_robot_arm: Operation successful
|
||||
|
||||
```
|
||||
#### 2）运行脚本后，运行轨迹从上至下如下图所示：
|
||||
|
||||
|
||||
|
||||
|
||||
## **6. 许可证信息**
|
||||
|
||||
* 本项目遵循MIT许可证。
|
||||
|
||||
## **7. 常见问题解答（FAQ）**
|
||||
|
||||
|
||||
- **Q:** 如何解决编译错误？
|
||||
**A:** 请确保您的编译器版本和依赖库满足系统要求，并按照安装说明重新配置环境。
|
||||
|
||||
- **Q:** 如何连接机器人？
|
||||
**A:** 请参考示例代码中的连接步骤，确保机器人IP地址和端口正确配置。
|
||||
|
||||
- **Q:** UDP数据推送接口收不到数据？
|
||||
**A:** 检查线程模式、是否使能推送数据、IP以及防火墙
|
||||
|
||||
- - **Q:** Trying to open file: C:/Users/dell/Videos/710/RMDemo_MovejCANFD/data/RM65&RM63_canfd_data.txt No valid points data found in file？
|
||||
**A:** #define ARM_DOF 6 在 RMDemo_MovejCANFD\include\utils.h 下是否未定义
|
||||
|
||||
|
||||
|
||||
27
HiveCoreR1/src/rm_arm_control/run.sh
Executable file
27
HiveCoreR1/src/rm_arm_control/run.sh
Executable file
@@ -0,0 +1,27 @@
|
||||
#!/bin/bash
|
||||
|
||||
# 设置构建目录
|
||||
BUILD_DIR=build
|
||||
|
||||
# 如果构建目录存在，则删除
|
||||
if [ -d "$BUILD_DIR" ]; then
|
||||
rm -rf $BUILD_DIR
|
||||
fi
|
||||
|
||||
# 创建构建目录
|
||||
mkdir $BUILD_DIR
|
||||
|
||||
# 进入构建目录
|
||||
cd $BUILD_DIR
|
||||
|
||||
# 运行CMake配置
|
||||
cmake ..
|
||||
|
||||
# 运行make进行编译
|
||||
make
|
||||
|
||||
# 返回根目录
|
||||
cd ..
|
||||
|
||||
# 运行编译后的可执行文件
|
||||
./build/RMDemo_MovejCANFD
|
||||
46
HiveCoreR1/src/rm_arm_control/src/rm_arm_handler.cpp
Normal file
46
HiveCoreR1/src/rm_arm_control/src/rm_arm_handler.cpp
Normal file
@@ -0,0 +1,46 @@
|
||||
#include "rm_arm_handler.hpp"
|
||||
|
||||
|
||||
RmArmHandler::RmArmHandler(RM_Service* roboticService, rm_robot_handle* robotHandle)
|
||||
{
|
||||
roboticService_ = *roboticService;
|
||||
robotHandle_ = *robotHandle;
|
||||
}
|
||||
|
||||
RmArmHandler::~RmArmHandler()
|
||||
{
|
||||
}
|
||||
|
||||
void RmArmHandler::CustomApiLog(const char* message, va_list args) {
|
||||
if (!message) {
|
||||
fprintf(stderr, "Error: message is a null pointer\n");
|
||||
return;
|
||||
}
|
||||
|
||||
char buffer[1024];
|
||||
vsnprintf(buffer, sizeof(buffer), message, args);
|
||||
printf(" %s\n", buffer);
|
||||
}
|
||||
|
||||
void RmArmHandler::ExecuteMovejCanfd(float points[ARM_DOF])
|
||||
{
|
||||
rm_robot_info_t robot_info;
|
||||
int info_result = roboticService_.rm_get_robot_info(&robotHandle_, &robot_info);
|
||||
if (info_result != 0) {
|
||||
printf("Failed to get robot info\n");
|
||||
return;
|
||||
}
|
||||
|
||||
int dof = robot_info.arm_dof;
|
||||
if (dof != ARM_DOF) {
|
||||
printf("Invalid degree of freedom\n");
|
||||
return;
|
||||
}
|
||||
|
||||
int result = roboticService_.rm_movej_canfd(&robotHandle_, points, false, 0);
|
||||
if (result != 0) {
|
||||
printf("Error moving to joint space! \n");
|
||||
}
|
||||
|
||||
printf("Pass-through completed\n");
|
||||
}
|
||||
183
HiveCoreR1/src/rm_arm_control/src/rm_arm_node.cpp
Normal file
183
HiveCoreR1/src/rm_arm_control/src/rm_arm_node.cpp
Normal file
@@ -0,0 +1,183 @@
|
||||
#include "rm_arm_node.hpp"
|
||||
|
||||
float RmArmNode::left_q_in_joint[ARM_DOF] = {0.0f};
|
||||
float RmArmNode::right_q_in_joint[ARM_DOF] = {0.0f};
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr RmArmNode::leftArmStatesPub_ = nullptr;
|
||||
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr RmArmNode::rightArmStatesPub_ = nullptr;
|
||||
|
||||
// 机器人控制器构造函数
|
||||
RmArmNode::RmArmNode() : Node("robot_Fsm_node")
|
||||
{
|
||||
rmService_ = new RM_Service();
|
||||
|
||||
rmService_->rm_set_log_call_back(custom_api_log, 3);
|
||||
rmService_->rm_realtime_arm_state_call_back(CallbackRmRealtimeArmJointState);
|
||||
|
||||
const char *robot_ip_address = "192.168.1.16";
|
||||
int robot_port = 8080;
|
||||
leftRmRobotHandler_ = rmService_->rm_create_robot_arm(robot_ip_address, robot_port);
|
||||
if (leftRmRobotHandler_ == NULL) {
|
||||
throw std::runtime_error("Failed to create left arm handle.");
|
||||
}
|
||||
leftArmHandler_ = new RmArmHandler(rmService_,leftRmRobotHandler_);
|
||||
|
||||
robot_ip_address = "192.168.1.18";
|
||||
robot_port = 8080;
|
||||
rightRmRobotHandler_ = rmService_->rm_create_robot_arm(robot_ip_address, robot_port);
|
||||
if (rightRmRobotHandler_ == NULL) {
|
||||
throw std::runtime_error("Failed to create right arm handle.");
|
||||
}
|
||||
rightArmHandler_ = new RmArmHandler(rmService_,rightRmRobotHandler_);
|
||||
|
||||
leftArmStatesPub_ = this->create_publisher<sensor_msgs::msg::JointState>("left_arm_states", 10);
|
||||
rightArmStatesPub_ = this->create_publisher<sensor_msgs::msg::JointState>("right_arm_states", 10);
|
||||
|
||||
leftArmCommandSub_ = this->create_subscription<std_msgs::msg::Float64MultiArray>("left_arm_command", 10, std::bind(&RmArmNode::leftArmCommandCallback, this, std::placeholders::_1));
|
||||
rightArmCommandSub_ = this->create_subscription<std_msgs::msg::Float64MultiArray>("right_arm_command", 10, std::bind(&RmArmNode::rightArmCommandCallback, this, std::placeholders::_1));
|
||||
|
||||
rm_realtime_push_config_t config = {100, true, 8089, 0, "192.168.1.66"};
|
||||
int result1 = rmService_-> rm_set_realtime_push(leftRmRobotHandler_, config);
|
||||
int result2 = rmService_-> rm_set_realtime_push(rightRmRobotHandler_, config);
|
||||
|
||||
if (result1 != 0 || result2 != 0) {
|
||||
throw std::runtime_error("Failed to set realtime push.");
|
||||
}
|
||||
}
|
||||
|
||||
RmArmNode::~RmArmNode()
|
||||
{
|
||||
rmService_ -> rm_delete_robot_arm(leftRmRobotHandler_);
|
||||
rmService_ -> rm_delete_robot_arm(rightRmRobotHandler_);
|
||||
|
||||
delete rmService_;
|
||||
delete leftArmHandler_;
|
||||
delete rightArmHandler_;
|
||||
}
|
||||
|
||||
void RmArmNode::CallbackRmRealtimeArmJointState(rm_realtime_arm_joint_state_t data)
|
||||
{
|
||||
printf("==================MYCALLBACK==================\n");
|
||||
printf("Current state: %.3f\n", data.joint_status.joint_current[0]);
|
||||
printf("Current angles: %.3f %.3f %.3f %.3f %.3f %.3f\n", data.joint_status.joint_position[0], data.joint_status.joint_position[1],
|
||||
data.joint_status.joint_position[2], data.joint_status.joint_position[3], data.joint_status.joint_position[4], data.joint_status.joint_position[5]);
|
||||
|
||||
printf("Current state: %u\n", data.joint_status.joint_err_code[0]);
|
||||
// Handle received data
|
||||
printf("Error Code: %d\n", data.errCode);
|
||||
printf("Arm IP: %s\n", data.arm_ip);
|
||||
|
||||
printf("Error: ");
|
||||
for (int i = 0; i < data.err.err_len; i++) {
|
||||
printf("%04x ", data.err.err[i]);
|
||||
}
|
||||
printf("\n");
|
||||
|
||||
// Handle joint state
|
||||
printf("Joint Position:\n");
|
||||
for (int i = 0; i < ARM_DOF; i++) {
|
||||
printf(" %.3f \n", data.joint_status.joint_position[i]);
|
||||
}
|
||||
|
||||
// Handle waypoint information
|
||||
printf("Waypoint:\n");
|
||||
printf(" Euler: [%.3f, %.3f, %.3f]\n", data.waypoint.euler.rx, data.waypoint.euler.ry, data.waypoint.euler.rz);
|
||||
printf(" Position: [%.3f, %.3f, %.3f]\n", data.waypoint.position.x, data.waypoint.position.y, data.waypoint.position.z);
|
||||
printf(" Quat: [%.3f, %.3f, %.3f, %.3f]\n", data.waypoint.quaternion.w, data.waypoint.quaternion.x, data.waypoint.quaternion.y, data.waypoint.quaternion.z);
|
||||
|
||||
|
||||
if (data.arm_ip == "192.168.1.16") {
|
||||
sensor_msgs::msg::JointState leftArmStates;
|
||||
leftArmStates.position = {data.joint_status.joint_position[0], data.joint_status.joint_position[1], data.joint_status.joint_position[2], data.joint_status.joint_position[3], data.joint_status.joint_position[4], data.joint_status.joint_position[5]};
|
||||
for (size_t i = 0; i < ARM_DOF; i++)
|
||||
{
|
||||
left_q_in_joint[i] = data.joint_status.joint_position[i];
|
||||
}
|
||||
leftArmStatesPub_->publish(leftArmStates);
|
||||
}
|
||||
else if (data.arm_ip == "192.168.1.18") {
|
||||
sensor_msgs::msg::JointState rightArmStates;
|
||||
rightArmStates.position = {data.joint_status.joint_position[0], data.joint_status.joint_position[1], data.joint_status.joint_position[2], data.joint_status.joint_position[3], data.joint_status.joint_position[4], data.joint_status.joint_position[5]};
|
||||
for (size_t i = 0; i < ARM_DOF; i++)
|
||||
{
|
||||
right_q_in_joint[i] = data.joint_status.joint_position[i];
|
||||
}
|
||||
rightArmStatesPub_->publish(rightArmStates);
|
||||
}
|
||||
}
|
||||
|
||||
void RmArmNode::custom_api_log(const char* message, va_list args)
|
||||
{
|
||||
if (!message) {
|
||||
fprintf(stderr, "Error: message is a null pointer\n");
|
||||
return;
|
||||
}
|
||||
|
||||
char buffer[1024];
|
||||
vsnprintf(buffer, sizeof(buffer), message, args);
|
||||
printf(" %s\n", buffer);
|
||||
}
|
||||
|
||||
void RmArmNode::leftArmCommandCallback(const std_msgs::msg::Float64MultiArray::SharedPtr msg)
|
||||
{
|
||||
// 创建一个float数组并复制数据
|
||||
std::vector<float> positions(*msg->data.begin(), *msg->data.end());
|
||||
|
||||
rm_inverse_kinematics_params_t inverse_params;
|
||||
float q_out[ARM_DOF] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
|
||||
memcpy(inverse_params.q_in, left_q_in_joint, sizeof(left_q_in_joint));
|
||||
inverse_params.q_pose.position.x = positions[0];
|
||||
inverse_params.q_pose.position.y = positions[1];
|
||||
inverse_params.q_pose.position.z = positions[2];
|
||||
inverse_params.q_pose.euler.rx = positions[3];
|
||||
inverse_params.q_pose.euler.ry = positions[4];
|
||||
inverse_params.q_pose.euler.rz = positions[5];
|
||||
inverse_params.flag = 1;
|
||||
int result = rm_algo_inverse_kinematics(NULL, inverse_params, q_out);
|
||||
|
||||
if (result != 0)
|
||||
{
|
||||
printf("Failed to calculate inverse kinematics.");
|
||||
return;
|
||||
}
|
||||
|
||||
leftArmHandler_->ExecuteMovejCanfd(q_out);
|
||||
}
|
||||
|
||||
void RmArmNode::rightArmCommandCallback(const std_msgs::msg::Float64MultiArray::SharedPtr msg)
|
||||
{
|
||||
// 创建一个float数组并复制数据
|
||||
std::vector<float> positions(*msg->data.begin(), *msg->data.end());
|
||||
|
||||
rm_inverse_kinematics_params_t inverse_params;
|
||||
float q_out[ARM_DOF] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
|
||||
memcpy(inverse_params.q_in, right_q_in_joint, sizeof(right_q_in_joint));
|
||||
inverse_params.q_pose.position.x = positions[0];
|
||||
inverse_params.q_pose.position.y = positions[1];
|
||||
inverse_params.q_pose.position.z = positions[2];
|
||||
inverse_params.q_pose.euler.rx = positions[3];
|
||||
inverse_params.q_pose.euler.ry = positions[4];
|
||||
inverse_params.q_pose.euler.rz = positions[5];
|
||||
inverse_params.flag = 1;
|
||||
int result = rm_algo_inverse_kinematics(NULL, inverse_params, q_out);
|
||||
|
||||
if (result != 0)
|
||||
{
|
||||
printf("Failed to calculate inverse kinematics.");
|
||||
return;
|
||||
}
|
||||
|
||||
rightArmHandler_->ExecuteMovejCanfd(positions.data());
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
auto robot_fsm_node = std::make_shared<RmArmNode>();
|
||||
|
||||
rclcpp::spin(robot_fsm_node);
|
||||
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
16
HiveCoreR1/src/robot_bringup/CMakeLists.txt
Normal file
16
HiveCoreR1/src/robot_bringup/CMakeLists.txt
Normal file
@@ -0,0 +1,16 @@
|
||||
cmake_minimum_required(VERSION 3.10)
|
||||
project(robot_bringup)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# 依赖
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
# 安装启动文件和世界文件
|
||||
install(DIRECTORY launch
|
||||
DESTINATION share/${PROJECT_NAME}/
|
||||
)
|
||||
|
||||
ament_package()
|
||||
202
HiveCoreR1/src/robot_bringup/LICENSE
Normal file
202
HiveCoreR1/src/robot_bringup/LICENSE
Normal file
@@ -0,0 +1,202 @@
|
||||
|
||||
Apache License
|
||||
Version 2.0, January 2004
|
||||
http://www.apache.org/licenses/
|
||||
|
||||
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||
|
||||
1. Definitions.
|
||||
|
||||
"License" shall mean the terms and conditions for use, reproduction,
|
||||
and distribution as defined by Sections 1 through 9 of this document.
|
||||
|
||||
"Licensor" shall mean the copyright owner or entity authorized by
|
||||
the copyright owner that is granting the License.
|
||||
|
||||
"Legal Entity" shall mean the union of the acting entity and all
|
||||
other entities that control, are controlled by, or are under common
|
||||
control with that entity. For the purposes of this definition,
|
||||
"control" means (i) the power, direct or indirect, to cause the
|
||||
direction or management of such entity, whether by contract or
|
||||
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||
|
||||
"You" (or "Your") shall mean an individual or Legal Entity
|
||||
exercising permissions granted by this License.
|
||||
|
||||
"Source" form shall mean the preferred form for making modifications,
|
||||
including but not limited to software source code, documentation
|
||||
source, and configuration files.
|
||||
|
||||
"Object" form shall mean any form resulting from mechanical
|
||||
transformation or translation of a Source form, including but
|
||||
not limited to compiled object code, generated documentation,
|
||||
and conversions to other media types.
|
||||
|
||||
"Work" shall mean the work of authorship, whether in Source or
|
||||
Object form, made available under the License, as indicated by a
|
||||
copyright notice that is included in or attached to the work
|
||||
(an example is provided in the Appendix below).
|
||||
|
||||
"Derivative Works" shall mean any work, whether in Source or Object
|
||||
form, that is based on (or derived from) the Work and for which the
|
||||
editorial revisions, annotations, elaborations, or other modifications
|
||||
represent, as a whole, an original work of authorship. For the purposes
|
||||
of this License, Derivative Works shall not include works that remain
|
||||
separable from, or merely link (or bind by name) to the interfaces of,
|
||||
the Work and Derivative Works thereof.
|
||||
|
||||
"Contribution" shall mean any work of authorship, including
|
||||
the original version of the Work and any modifications or additions
|
||||
to that Work or Derivative Works thereof, that is intentionally
|
||||
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||
or by an individual or Legal Entity authorized to submit on behalf of
|
||||
the copyright owner. For the purposes of this definition, "submitted"
|
||||
means any form of electronic, verbal, or written communication sent
|
||||
to the Licensor or its representatives, including but not limited to
|
||||
communication on electronic mailing lists, source code control systems,
|
||||
and issue tracking systems that are managed by, or on behalf of, the
|
||||
Licensor for the purpose of discussing and improving the Work, but
|
||||
excluding communication that is conspicuously marked or otherwise
|
||||
designated in writing by the copyright owner as "Not a Contribution."
|
||||
|
||||
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||
on behalf of whom a Contribution has been received by Licensor and
|
||||
subsequently incorporated within the Work.
|
||||
|
||||
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
copyright license to reproduce, prepare Derivative Works of,
|
||||
publicly display, publicly perform, sublicense, and distribute the
|
||||
Work and such Derivative Works in Source or Object form.
|
||||
|
||||
3. Grant of Patent License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
(except as stated in this section) patent license to make, have made,
|
||||
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||
where such license applies only to those patent claims licensable
|
||||
by such Contributor that are necessarily infringed by their
|
||||
Contribution(s) alone or by combination of their Contribution(s)
|
||||
with the Work to which such Contribution(s) was submitted. If You
|
||||
institute patent litigation against any entity (including a
|
||||
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||
or a Contribution incorporated within the Work constitutes direct
|
||||
or contributory patent infringement, then any patent licenses
|
||||
granted to You under this License for that Work shall terminate
|
||||
as of the date such litigation is filed.
|
||||
|
||||
4. Redistribution. You may reproduce and distribute copies of the
|
||||
Work or Derivative Works thereof in any medium, with or without
|
||||
modifications, and in Source or Object form, provided that You
|
||||
meet the following conditions:
|
||||
|
||||
(a) You must give any other recipients of the Work or
|
||||
Derivative Works a copy of this License; and
|
||||
|
||||
(b) You must cause any modified files to carry prominent notices
|
||||
stating that You changed the files; and
|
||||
|
||||
(c) You must retain, in the Source form of any Derivative Works
|
||||
that You distribute, all copyright, patent, trademark, and
|
||||
attribution notices from the Source form of the Work,
|
||||
excluding those notices that do not pertain to any part of
|
||||
the Derivative Works; and
|
||||
|
||||
(d) If the Work includes a "NOTICE" text file as part of its
|
||||
distribution, then any Derivative Works that You distribute must
|
||||
include a readable copy of the attribution notices contained
|
||||
within such NOTICE file, excluding those notices that do not
|
||||
pertain to any part of the Derivative Works, in at least one
|
||||
of the following places: within a NOTICE text file distributed
|
||||
as part of the Derivative Works; within the Source form or
|
||||
documentation, if provided along with the Derivative Works; or,
|
||||
within a display generated by the Derivative Works, if and
|
||||
wherever such third-party notices normally appear. The contents
|
||||
of the NOTICE file are for informational purposes only and
|
||||
do not modify the License. You may add Your own attribution
|
||||
notices within Derivative Works that You distribute, alongside
|
||||
or as an addendum to the NOTICE text from the Work, provided
|
||||
that such additional attribution notices cannot be construed
|
||||
as modifying the License.
|
||||
|
||||
You may add Your own copyright statement to Your modifications and
|
||||
may provide additional or different license terms and conditions
|
||||
for use, reproduction, or distribution of Your modifications, or
|
||||
for any such Derivative Works as a whole, provided Your use,
|
||||
reproduction, and distribution of the Work otherwise complies with
|
||||
the conditions stated in this License.
|
||||
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
|
||||
6. Trademarks. This License does not grant permission to use the trade
|
||||
names, trademarks, service marks, or product names of the Licensor,
|
||||
except as required for reasonable and customary use in describing the
|
||||
origin of the Work and reproducing the content of the NOTICE file.
|
||||
|
||||
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||
agreed to in writing, Licensor provides the Work (and each
|
||||
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||
implied, including, without limitation, any warranties or conditions
|
||||
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||
appropriateness of using or redistributing the Work and assume any
|
||||
risks associated with Your exercise of permissions under this License.
|
||||
|
||||
8. Limitation of Liability. In no event and under no legal theory,
|
||||
whether in tort (including negligence), contract, or otherwise,
|
||||
unless required by applicable law (such as deliberate and grossly
|
||||
negligent acts) or agreed to in writing, shall any Contributor be
|
||||
liable to You for damages, including any direct, indirect, special,
|
||||
incidental, or consequential damages of any character arising as a
|
||||
result of this License or out of the use or inability to use the
|
||||
Work (including but not limited to damages for loss of goodwill,
|
||||
work stoppage, computer failure or malfunction, or any and all
|
||||
other commercial damages or losses), even if such Contributor
|
||||
has been advised of the possibility of such damages.
|
||||
|
||||
9. Accepting Warranty or Additional Liability. While redistributing
|
||||
the Work or Derivative Works thereof, You may choose to offer,
|
||||
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||
or other liability obligations and/or rights consistent with this
|
||||
License. However, in accepting such obligations, You may act only
|
||||
on Your own behalf and on Your sole responsibility, not on behalf
|
||||
of any other Contributor, and only if You agree to indemnify,
|
||||
defend, and hold each Contributor harmless for any liability
|
||||
incurred by, or claims asserted against, such Contributor by reason
|
||||
of your accepting any such warranty or additional liability.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
APPENDIX: How to apply the Apache License to your work.
|
||||
|
||||
To apply the Apache License to your work, attach the following
|
||||
boilerplate notice, with the fields enclosed by brackets "[]"
|
||||
replaced with your own identifying information. (Don't include
|
||||
the brackets!) The text should be enclosed in the appropriate
|
||||
comment syntax for the file format. We also recommend that a
|
||||
file or class name and description of purpose be included on the
|
||||
same "printed page" as the copyright notice for easier
|
||||
identification within third-party archives.
|
||||
|
||||
Copyright [yyyy] [name of copyright owner]
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
21
HiveCoreR1/src/robot_bringup/launch/main.launch.py
Normal file
21
HiveCoreR1/src/robot_bringup/launch/main.launch.py
Normal file
@@ -0,0 +1,21 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch.substitutions import PathJoinSubstitution
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
rm_Arm_pkg_share = FindPackageShare(package="rm_arm_control")
|
||||
rm_Arm_launch_path = PathJoinSubstitution(
|
||||
[rm_Arm_pkg_share, "launch", "rm_arm_control.launch.py"]
|
||||
)
|
||||
|
||||
|
||||
include_rm_Arm_control = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(rm_Arm_launch_path),
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
include_rm_Arm_control,
|
||||
])
|
||||
15
HiveCoreR1/src/robot_bringup/package.xml
Normal file
15
HiveCoreR1/src/robot_bringup/package.xml
Normal file
@@ -0,0 +1,15 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>robot_bringup</name>
|
||||
<version>1.0.0</version>
|
||||
<description>Robot control package</description>
|
||||
<maintainer email="Ray@email.com">Your Name</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<build_depend>ament_cmake</build_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -1,27 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(robot_simulation)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(gazebo_ros REQUIRED)
|
||||
find_package(gazebo_plugins REQUIRED)
|
||||
find_package(urdf REQUIRED)
|
||||
find_package(xacro REQUIRED)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
@@ -1,68 +0,0 @@
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
|
||||
def generate_launch_description():
|
||||
# 设置参数
|
||||
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
|
||||
world_file_name = 'empty.world'
|
||||
world_path = os.path.join(get_package_share_directory('robot_simulation'), 'worlds', world_file_name)
|
||||
|
||||
# URDF文件路径
|
||||
urdf_file_name = 'robot.urdf.xacro'
|
||||
urdf_path = os.path.join(get_package_share_directory('robot_simulation'), 'urdf', urdf_file_name)
|
||||
|
||||
# 启动Gazebo服务器
|
||||
gazebo_server = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([
|
||||
os.path.join(get_package_share_directory('gazebo_ros'), 'launch'),
|
||||
'/gzserver.launch.py'
|
||||
]),
|
||||
launch_arguments={'world': world_path}.items()
|
||||
)
|
||||
|
||||
# 启动Gazebo客户端
|
||||
gazebo_client = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([
|
||||
os.path.join(get_package_share_directory('gazebo_ros'), 'launch'),
|
||||
'/gzclient.launch.py'
|
||||
])
|
||||
)
|
||||
|
||||
# 启动机器人状态发布器
|
||||
robot_state_publisher = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
output='screen',
|
||||
parameters=[{'use_sim_time': use_sim_time, 'robot_description': open(urdf_path, 'r').read()}]
|
||||
)
|
||||
|
||||
# 启动关节状态发布器
|
||||
joint_state_publisher = Node(
|
||||
package='joint_state_publisher',
|
||||
executable='joint_state_publisher',
|
||||
name='joint_state_publisher',
|
||||
output='screen',
|
||||
parameters=[{'use_sim_time': use_sim_time}]
|
||||
)
|
||||
|
||||
# 在Gazebo中生成机器人
|
||||
spawn_entity = Node(
|
||||
package='gazebo_ros',
|
||||
executable='spawn_entity.py',
|
||||
arguments=['-entity', 'my_robot', '-topic', 'robot_description'],
|
||||
output='screen'
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
gazebo_server,
|
||||
gazebo_client,
|
||||
robot_state_publisher,
|
||||
joint_state_publisher,
|
||||
spawn_entity
|
||||
])
|
||||
@@ -1,23 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>robot_simulation</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="ray@todo.todo">ray</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>gazebo_ros</depend>
|
||||
<depend>gazebo_plugins</depend>
|
||||
<depend>urdf</depend>
|
||||
<depend>xacro</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -1,150 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot name="my_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<!-- 定义常量 -->
|
||||
<xacro:property name="M_PI" value="3.141592653589793"/>
|
||||
|
||||
<!-- 机器人底座 -->
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="blue">
|
||||
<color rgba="0 0 1.0 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.2"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10.0"/>
|
||||
<inertia ixx="0.2" ixy="0.0" ixz="0.0" iyy="0.2" iyz="0.0" izz="0.2"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<!-- 轮子 -->
|
||||
<xacro:macro name="wheel" params="prefix x y z">
|
||||
<link name="${prefix}_wheel_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.075"/>
|
||||
</geometry>
|
||||
<material name="gray">
|
||||
<color rgba="0.5 0.5 0.5 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.075"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.0"/>
|
||||
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.002"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="${prefix}_wheel_joint" type="continuous">
|
||||
<origin xyz="${x} ${y} ${z}" rpy="0 ${M_PI/2} 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="${prefix}_wheel_link"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0.1" friction="0.01"/>
|
||||
</joint>
|
||||
</xacro:macro>
|
||||
|
||||
<!-- 创建四个轮子 -->
|
||||
<xacro:wheel prefix="front_left" x="0.15" y="0.2" z="-0.05"/>
|
||||
<xacro:wheel prefix="front_right" x="0.15" y="-0.2" z="-0.05"/>
|
||||
<xacro:wheel prefix="rear_left" x="-0.15" y="0.2" z="-0.05"/>
|
||||
<xacro:wheel prefix="rear_right" x="-0.15" y="-0.2" z="-0.05"/>
|
||||
|
||||
<!-- 激光雷达 -->
|
||||
<link name="laser_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.05"/>
|
||||
</geometry>
|
||||
<material name="yellow">
|
||||
<color rgba="1.0 1.0 0 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0002"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="laser_joint" type="fixed">
|
||||
<origin xyz="0 0 0.15" rpy="0 0 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="laser_link"/>
|
||||
</joint>
|
||||
|
||||
<!-- Gazebo插件 -->
|
||||
<!-- 差速驱动插件 -->
|
||||
<gazebo>
|
||||
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
|
||||
<ros>
|
||||
<namespace>/</namespace>
|
||||
<param name="cmd_vel_topic">cmd_vel</param>
|
||||
<param name="odom_topic">odom</param>
|
||||
<param name="odom_frame">odom</param>
|
||||
<param name="base_frame">base_link</param>
|
||||
</ros>
|
||||
<wheel_separation>0.4</wheel_separation>
|
||||
<wheel_diameter>0.15</wheel_diameter>
|
||||
<update_rate>100.0</update_rate>
|
||||
<publish_tf>1</publish_tf>
|
||||
<odometry_frame>odom</odometry_frame>
|
||||
<odometry_topic>odom</odometry_topic>
|
||||
<robot_base_frame>base_link</robot_base_frame>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
|
||||
<!-- 激光雷达插件 -->
|
||||
<gazebo reference="laser_link">
|
||||
<sensor type="ray" name="laser_sensor">
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<visualize>true</visualize>
|
||||
<update_rate>20.0</update_rate>
|
||||
<ray>
|
||||
<scan>
|
||||
<horizontal>
|
||||
<samples>360</samples>
|
||||
<resolution>1</resolution>
|
||||
<min_angle>-3.141592653589793</min_angle>
|
||||
<max_angle>3.141592653589793</max_angle>
|
||||
</horizontal>
|
||||
</scan>
|
||||
<range>
|
||||
<min>0.1</min>
|
||||
<max>30.0</max>
|
||||
<resolution>0.01</resolution>
|
||||
</range>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</ray>
|
||||
<plugin name="gazebo_ros_laser" filename="libgazebo_ros_laser.so">
|
||||
<ros>
|
||||
<namespace>/</namespace>
|
||||
<param name="topic">scan</param>
|
||||
</ros>
|
||||
<frameName>laser_link</frameName>
|
||||
<topicName>scan</topicName>
|
||||
<visualize>true</visualize>
|
||||
<updateRateHZ>20.0</updateRateHZ>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
</robot>
|
||||
Reference in New Issue
Block a user