catch_child.cpp

博客涉及C++、C语言以及Linux相关内容,虽未给出具体内容,但推测围绕C++和C语言在Linux环境下的开发、应用等信息技术方面展开。
/*
 * function: 父进程使用sigaction()函数捕捉SIGCHLD信号子进程
 *
 * 2020-12-27
 */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <signal.h>
#include <sys/wait.h>

void func(int signo)
{
    pid_t wpid;
    int status;

    while ((wpid = waitpid(-1, &status, 0)) != -1)         // 阻塞回收, -1表示没有子进程
    {
        if (WIFEXITED(status))
        {
            printf("catch a child %d, normal exit status:%d\n", wpid,  WEXITSTATUS(status));
        }
        else if (WIFSIGNALED(status))
        {
            printf("catch a child %d, kill by signal:%d\n", wpid, WTERMSIG(status));
        }
        else if (WCOREDUMP(status))
        {
            printf("catch a child %d, stop status:%d\n", wpid, WIFSTOPPED(status));
        }
    }
}

int main(int argc, char *argv[])
{
    pid_t pid = 0;

    // 阻塞SIGCHLD信号,防止在父进程注册捕捉函数之前子进程退出
    sigset_t set;
    sigemptyset(&set);
    sigaddset(&set, SIGCHLD);
    sigprocmask(SIG_BLOCK, &set, NULL);

    int ii = 0;
    for (ii = 0; ii < 5; ++ii)
    {
        pid = fork();
        if (0 == pid)
        {
            break;      // 子进程跳出循环
        }
        else if (-1 == pid)
        {
            perror("fork failed");
            exit(1);
        }
    }

    if (pid > 0)    // 父进程
    {
        printf("I'm parent:%d\n", getpid());

        struct sigaction act;
        act.sa_handler = func;       // 设置回调函数
        sigemptyset(&act.sa_mask);  // 清空sa_mask
        act.sa_flags = 0;           // 设置默认属性

        sigaction(SIGCHLD, &act, NULL);     // 注册信号捕捉

        // 解除阻塞
        sigprocmask(SIG_UNBLOCK, &set, NULL);

        while(1);       // 模拟父进程后续操作
    }
    else if (0 == pid)   // 子进程
    {
        printf("I'm a child:%d\n", getpid());
        return ii;
    }

    return 0;
}
“#include "wall_detection.h" // 全局变量定义 tf2_ros::Buffer* tfBuffer = nullptr; ros::Publisher pointcloud_pub; ros::Publisher lines_pub; ros::Publisher normals_pub; ros::Publisher nearest_point_pub; ros::Publisher tf_points_pub; ros::Publisher min_distance_pub; ros::Publisher nearest_distance_pub; //最近距离 // ros::Publisher left_front_nearest_distance_pub; // 左前侧垂直距离 ros::Publisher right_front_nearest_distance_pub; // 右前侧垂直距离 ros::Publisher left_back_nearest_distance_pub; // 左后侧垂直距离 ros::Publisher right_back_nearest_distance_pub; // 右后侧垂直距离 ros::Publisher foot_points_pub;//垂足 // bool isLeftWall(const DetectedLine& wall) { // 计算墙面中点Y坐标,大于0视为左侧墙面 return (wall.start.y + wall.end.y) / 2.0 > 0; } geometry_msgs::Point left_front_ref, left_back_ref; geometry_msgs::Point right_front_ref, right_back_ref; std::string choose_wall ; bool getPointParameters(ros::NodeHandle& nh) { // 为每个点的每个分量设置默认值 double left_front_default_x = 0.0, left_front_default_y = 0.0, left_front_default_z = 0.0; double left_back_default_x = 0.0, left_back_default_y = 0.0, left_back_default_z = 0.0; double right_front_default_x = 0.0, right_front_default_y = 0.0, right_front_default_z = 0.0; double right_back_default_x = 0.0, right_back_default_y = 0.0, right_back_default_z = 0.0; std::string inital_choose = "all"; // 获取左前点参数 nh.param<double>("left_front_ref_x", left_front_ref.x, left_front_default_x); nh.param<double>("left_front_ref_y", left_front_ref.y, left_front_default_y); nh.param<double>("left_front_ref_z", left_front_ref.z, left_front_default_z); // 获取左后点参数 nh.param<double>("left_back_ref_x", left_back_ref.x, left_back_default_x); nh.param<double>("left_back_ref_y", left_back_ref.y, left_back_default_y); nh.param<double>("left_back_ref_z", left_back_ref.z, left_back_default_z); // 获取右前点参数 nh.param<double>("right_front_ref_x", right_front_ref.x, right_front_default_x); nh.param<double>("right_front_ref_y", right_front_ref.y, right_front_default_y); nh.param<double>("right_front_ref_z", right_front_ref.z, right_front_default_z); // 获取右后点参数 nh.param<double>("right_back_ref_x", right_back_ref.x, right_back_default_x); nh.param<double>("right_back_ref_y", right_back_ref.y, right_back_default_y); nh.param<double>("right_back_ref_z", right_back_ref.z, right_back_default_z); nh.param<std::string>("choose_wall",choose_wall, "inital_choose"); // 打印获取的值 ROS_INFO("Left Front Ref: (%.2f, %.2f, %.2f)", left_front_ref.x, left_front_ref.y, left_front_ref.z); ROS_INFO("Left Back Ref: (%.2f, %.2f, %.2f)", left_back_ref.x, left_back_ref.y, left_back_ref.z); ROS_INFO("Right Front Ref: (%.2f, %.2f, %.2f)", right_front_ref.x, right_front_ref.y, right_front_ref.z); ROS_INFO("Right Back Ref: (%.2f, %.2f, %.2f)", right_back_ref.x, right_back_ref.y, right_back_ref.z); ROS_INFO("Choose_wall_type: %s", choose_wall.c_str()); return true; } void setupStaticTFs() { static tf2_ros::StaticTransformBroadcaster static_broadcaster; // 位于激光雷达左前方的tf坐标 geometry_msgs::TransformStamped left_tf; //left_tf.header.stamp = ros::Time::now(); left_tf.header.stamp = ros::Time(0); left_tf.header.frame_id = "base_footprint"; left_tf.child_frame_id = "laser_left_front"; left_tf.transform.translation.x = left_front_ref.x; left_tf.transform.translation.y = left_front_ref.y; left_tf.transform.translation.z = left_front_ref.z; left_tf.transform.rotation.x = 0.0; left_tf.transform.rotation.y = 0.0; left_tf.transform.rotation.z = 0.0; left_tf.transform.rotation.w = 1.0; // 位于激光雷达右前方的tf坐标 geometry_msgs::TransformStamped right_tf; //right_tf.header.stamp = ros::Time::now(); right_tf.header.stamp = ros::Time(0); right_tf.header.frame_id = "base_footprint"; right_tf.child_frame_id = "laser_right_front"; right_tf.transform.translation.x = right_front_ref.x; right_tf.transform.translation.y = right_front_ref.y; right_tf.transform.translation.z = right_front_ref.z; right_tf.transform.rotation.x = 0.0; right_tf.transform.rotation.y = 0.0; right_tf.transform.rotation.z = 0.0; right_tf.transform.rotation.w = 1.0; //左后方TF坐标 geometry_msgs::TransformStamped left_back_tf; //left_back_tf.header.stamp = ros::Time::now(); left_back_tf.header.stamp = ros::Time(0); left_back_tf.header.frame_id = "base_footprint"; left_back_tf.child_frame_id = "laser_left_back"; left_back_tf.transform.translation.x = left_back_ref.x; left_back_tf.transform.translation.y = left_back_ref.y; left_back_tf.transform.translation.z = left_back_ref.z; left_back_tf.transform.rotation.x = 0.0; left_back_tf.transform.rotation.y = 0.0; left_back_tf.transform.rotation.z = 0.0; left_back_tf.transform.rotation.w = 1.0; // 右后方TF坐标 geometry_msgs::TransformStamped right_back_tf; //right_back_tf.header.stamp = ros::Time::now(); right_back_tf.header.stamp = ros::Time(0); right_back_tf.header.frame_id = "base_footprint"; right_back_tf.child_frame_id = "laser_right_back"; right_back_tf.transform.translation.x = right_back_ref.x; right_back_tf.transform.translation.y = right_back_ref.y; right_back_tf.transform.translation.z = right_back_ref.z; right_back_tf.transform.rotation.x = 0.0; right_back_tf.transform.rotation.y = 0.0; right_back_tf.transform.rotation.z = 0.0; right_back_tf.transform.rotation.w = 1.0; static_broadcaster.sendTransform(left_tf); static_broadcaster.sendTransform(right_tf); static_broadcaster.sendTransform(left_back_tf); static_broadcaster.sendTransform(right_back_tf); } double pointToLineDistance(const geometry_msgs::Point& point, const geometry_msgs::Point& line_start, const geometry_msgs::Point& line_end) { Eigen::Vector3f pt(point.x, point.y, point.z); Eigen::Vector3f line_vec(line_end.x - line_start.x, line_end.y - line_start.y, line_end.z - line_start.z); Eigen::Vector3f pt_vec(pt.x() - line_start.x, pt.y() - line_start.y, pt.z() - line_start.z); double line_length = line_vec.norm(); if (line_length < 1e-6) { return pt_vec.norm(); } Eigen::Vector3f normalized_line = line_vec / line_length; double projection = pt_vec.dot(normalized_line); // 限制投影在直线范围内 projection = std::max(0.0, std::min(line_length, projection)); Eigen::Vector3f closest_point = Eigen::Vector3f(line_start.x, line_start.y, line_start.z) + projection * normalized_line; return (pt - closest_point).norm(); } // 计算点到直线的垂足点 geometry_msgs::Point pointToLineProjection(const geometry_msgs::Point& point, const geometry_msgs::Point& line_start, const geometry_msgs::Point& line_end) { Eigen::Vector3f pt(point.x, point.y, point.z); Eigen::Vector3f line_vec(line_end.x - line_start.x, line_end.y - line_start.y, line_end.z - line_start.z); Eigen::Vector3f pt_vec(pt.x() - line_start.x, pt.y() - line_start.y, pt.z() - line_start.z); double line_length = line_vec.norm(); if (line_length < 1e-6) { return line_start; // 如果直线长度为零,返回起点 } Eigen::Vector3f normalized_line = line_vec / line_length; double projection = pt_vec.dot(normalized_line); // 限制投影在直线范围内 projection = std::max(0.0, std::min(line_length, projection)); Eigen::Vector3f closest_point = Eigen::Vector3f(line_start.x, line_start.y, line_start.z) + projection * normalized_line; geometry_msgs::Point foot_point; foot_point.x = closest_point.x(); foot_point.y = closest_point.y(); foot_point.z = closest_point.z(); return foot_point; } void publishDetectedLines(const std::vector<DetectedLine>& lines, const std::string& frame_id) { visualization_msgs::Marker line_marker; line_marker.header.frame_id = frame_id; line_marker.header.stamp = ros::Time::now(); line_marker.ns = "detected_line"; line_marker.id = 0; line_marker.type = visualization_msgs::Marker::LINE_LIST; line_marker.pose.orientation.w = 1.0; line_marker.scale.x = 0.15; line_marker.color.r = 0.0; line_marker.color.g = 0.0; line_marker.color.b = 1.0; line_marker.color.a = 1.0; for (const auto& line : lines) { geometry_msgs::Point p1, p2; p1.x = line.start.x; p1.y = line.start.y; p1.z = line.start.z; p2.x = line.end.x; p2.y = line.end.y; p2.z = line.end.z; line_marker.points.push_back(p1); line_marker.points.push_back(p2); } lines_pub.publish(line_marker); visualization_msgs::MarkerArray normal_markers; int id = 0; for (const auto& line : lines) { visualization_msgs::Marker normal_marker; normal_marker.header.frame_id = frame_id; normal_marker.header.stamp = ros::Time::now(); normal_marker.ns = "normals"; normal_marker.id = id++; normal_marker.type = visualization_msgs::Marker::ARROW; normal_marker.action = visualization_msgs::Marker::ADD; normal_marker.pose.orientation.w = 1.0; normal_marker.scale.x = 0.02; normal_marker.scale.y = 0.04; normal_marker.scale.z = 0.0; normal_marker.color.r = 1.0; normal_marker.color.g = 0.0; normal_marker.color.b = 0.0; normal_marker.color.a = 1.0; geometry_msgs::Point mid_point; mid_point.x = (line.start.x + line.end.x) / 2.0; mid_point.y = (line.start.y + line.end.y) / 2.0; mid_point.z = (line.start.z + line.end.z) / 2.0; geometry_msgs::Point normal_end; normal_end.x = mid_point.x + line.normal.normal_x * 0.5; normal_end.y = mid_point.y + line.normal.normal_y * 0.5; normal_end.z = mid_point.z + line.normal.normal_z * 0.5; normal_marker.points.push_back(mid_point); normal_marker.points.push_back(normal_end); normal_markers.markers.push_back(normal_marker); } normals_pub.publish(normal_markers); } void publishNearestPointMarker(const geometry_msgs::Point& point,float distance,const std::string& frame_id, const std::string& ref_name) { visualization_msgs::Marker marker; marker.header.frame_id = frame_id; marker.header.stamp = ros::Time::now(); marker.ns = "nearest_point" + ref_name; // marker.id = 0; marker.type = visualization_msgs::Marker::SPHERE; marker.action = visualization_msgs::Marker::ADD; marker.pose.position = point; marker.pose.orientation.w = 1.0; marker.scale.x = 0.15; marker.scale.y = 0.15; marker.scale.z = 0.15; marker.color.r = 1.0; marker.color.g = 0.0; marker.color.b = 0.0; marker.color.a = 1.0; marker.lifetime = ros::Duration(0.1); nearest_point_pub.publish(marker); //std_msgs::Float64 dist; //dist.data =(point.y < 0 ? -1.0 : 1.0) *(std::fabs(point.y) - 0.12); //dist.data = std::fabs(point.y); //nearest_distance_pub.publish(dist); //最近距离 std_msgs::Float64 dist_msg; if (ref_name == "left_front") { dist_msg.data = distance; left_front_nearest_distance_pub.publish(dist_msg); } else if (ref_name == "left_back") { dist_msg.data = distance; left_back_nearest_distance_pub.publish(dist_msg); } else if (ref_name == "right_front") { dist_msg.data = distance; right_front_nearest_distance_pub.publish(dist_msg); } else { dist_msg.data = distance; right_back_nearest_distance_pub.publish(dist_msg); } // } /***************************************************************/ void publishFootPointMarker(const geometry_msgs::Point& left_front_foot, //垂足标记 const geometry_msgs::Point& left_back_foot, //垂足标记 const geometry_msgs::Point& right_front_foot, const geometry_msgs::Point& right_back_foot, const std::string& frame_id) { visualization_msgs::Marker marker; marker.header.frame_id = frame_id; marker.header.stamp = ros::Time::now(); marker.ns = "foot_points"; marker.id = 0; marker.type = visualization_msgs::Marker::POINTS; marker.action = visualization_msgs::Marker::ADD; marker.scale.x = 0.1; // 点的大小 marker.scale.y = 0.1; marker.color.a = 1.0; // 不透明度 // 添加左侧垂足点 - 蓝色 marker.points.push_back(left_front_foot); std_msgs::ColorRGBA color; color.r = 0.0; color.g = 0.0; color.b = 1.0; color.a = 1.0; marker.colors.push_back(color); // 添加右侧垂足点 - 绿色 marker.points.push_back(right_front_foot); color.r = 0.0; color.g = 1.0; color.b = 0.0; color.a = 1.0; marker.colors.push_back(color); // 添加左侧垂足点 - 蓝色 marker.points.push_back(left_back_foot); color.r = 0.0; color.g = 1.0; color.b = 1.0; color.a = 1.0; marker.colors.push_back(color); // 添加右侧垂足点 - 绿色 marker.points.push_back(right_back_foot); color.r = 1.0; color.g = 1.0; color.b = 0.0; color.a = 1.0; marker.colors.push_back(color); foot_points_pub.publish(marker); } /*******************************************************************/ void publishTFPoints(const geometry_msgs::Point& left_front_point, const geometry_msgs::Point& left_back_point, const geometry_msgs::Point& right_front_point, const geometry_msgs::Point& right_back_point, const std::string& frame_id) { visualization_msgs::Marker marker; marker.header.frame_id = frame_id; marker.header.stamp = ros::Time::now(); marker.ns = "tf_point"; marker.id = 0; marker.type = visualization_msgs::Marker::POINTS; marker.action = visualization_msgs::Marker::ADD; marker.scale.x = 0.15; marker.scale.y = 0.15; marker.color.a = 1.0; // 左侧点 - 蓝色 geometry_msgs::Point p; p = left_front_point; marker.points.push_back(p); std_msgs::ColorRGBA c; c.r = 0.0; c.g = 0.0; c.b = 1.0; c.a = 1.0; marker.colors.push_back(c); // 右侧点 - 绿色 p = right_front_point; marker.points.push_back(p); c.r = 0.0; c.g = 1.0; c.b = 0.0; c.a = 1.0; marker.colors.push_back(c); // 左侧点 - 蓝色 p = left_back_point; marker.points.push_back(p); c.r = 0.0; c.g = 1.0; c.b = 1.0; c.a = 1.0; marker.colors.push_back(c); // 右侧点 - 绿色 p = right_back_point; marker.points.push_back(p); c.r = 1.0; c.g = 1.0; c.b = 0.0; c.a = 1.0; marker.colors.push_back(c); tf_points_pub.publish(marker); } void publishDistanceInfo(const std::string& frame_id, double left_front_dist, double left_back_dist, double right_front_dist, double right_back_dist, const geometry_msgs::Point& wall_point) { visualization_msgs::Marker marker; marker.header.frame_id = frame_id; marker.header.stamp = ros::Time::now(); marker.ns = "distance_info"; marker.id = 0; marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; marker.action = visualization_msgs::Marker::ADD; marker.pose.position = wall_point; marker.pose.position.z += 0.5; // 在墙上点上方显示 marker.pose.orientation.w = 1.0; marker.scale.z = 0.2; // 文本大小 marker.color.r = 1.0; marker.color.g = 1.0; marker.color.b = 0.0; marker.color.a = 1.0; std::stringstream ss; ss << std::fixed << std::setprecision(2); ss << "Left_front_tf: " << left_front_dist << "m\n"; ss << "Left_back_tf: " << left_back_dist << "m\n"; ss << "Right_front_tf: " << right_front_dist << "m\n"; ss << "Right_back_tf: " << right_back_dist << "m\n"; if (left_front_dist < right_front_dist && left_back_dist < right_back_dist ) { ss << "Left is closer"; } else if (right_front_dist < left_front_dist && right_back_dist < left_back_dist ) { ss << "Right is closer"; } else { ss << "Equal distance"; } marker.text = ss.str(); min_distance_pub.publish(marker); } float min_four(float a ,float b , float c , float d ) { return std::min({a, b, c, d}); } void LidarCallback(const sensor_msgs::LaserScan::ConstPtr& msg) { std::unordered_map<int, PointData> point_data_map; PointCloudT::Ptr cloud(new PointCloudT); cloud->header.frame_id = msg->header.frame_id; cloud->height = 1; cloud->is_dense = false; float min_distance = std::numeric_limits<float>::max(); int min_index = -1; int point_count = 0; const float min_angle1 = 30 * M_PI/180.0; const float max_angle1 = 150* M_PI/180.0; const float min_angle2 = -150 * M_PI/180.0; const float max_angle2 = -30 * M_PI/180.0; // 为参考点初始化最小距离 float left_front_min_distance = std::numeric_limits<float>::max(); int left_front_min_index = -1; geometry_msgs::Point left_front_nearest_point; float left_back_min_distance = std::numeric_limits<float>::max(); int left_back_min_index = -1; geometry_msgs::Point left_back_nearest_point; // 为参考点初始化最小距离 float right_front_min_distance = std::numeric_limits<float>::max(); int right_front_min_index = -1; geometry_msgs::Point right_front_nearest_point; float right_back_min_distance = std::numeric_limits<float>::max(); int right_back_min_index = -1; geometry_msgs::Point right_back_nearest_point; // 寻找最近点 for (size_t i = 0; i < msg->ranges.size(); ++i) { const float range = msg->ranges[i]; if (std::isnan(range)) continue; if (range < msg->range_min || range > msg->range_max) continue; const float angle = msg -> angle_min + i * msg -> angle_increment; //创建屏蔽条件检测 bool in_blocked_zone = true; float normalized_angle = angle; const float x = range * cos(angle); const float y = range * sin(angle); if (choose_wall == "left") { // 左侧 if (angle >= min_angle1 && angle <= max_angle1) { // if ( x > left_front_ref.x || x < left_back_ref.x) // { // in_blocked_zone= false; // } in_blocked_zone= false; } } else if (choose_wall == "right") { // 右侧 if (angle >= min_angle2 && angle <= max_angle2) { // if ( x > right_front_ref.x || x < right_back_ref.x) // { // in_blocked_zone= false; // } in_blocked_zone= false; } } else { if (angle >= min_angle1 && angle <= max_angle1) { // if ( x > left_front_ref.x || x < left_back_ref.x) // { // in_blocked_zone= false; // } in_blocked_zone= false; } if (angle >= min_angle2 && angle <= max_angle2) { // if ( x > right_front_ref.x || x < right_back_ref.x) // { // in_blocked_zone= false; // } in_blocked_zone= false; } } if (in_blocked_zone) continue; if (range < min_distance) { min_distance = range; min_index = i; } PointT point; point.x = range * cos(angle); point.y = range * sin(angle); point.z = 0.0; point.r = 0; point.g = 255; point.b = 0; PointData data; data.original_index = i; data.is_line_point = false; data.is_nearest = (i == min_index); point_data_map[point_count] = data; cloud->points.push_back(point); point_count++; } cloud->width = point_count; // 如果点云为空,直接返回 if (cloud->empty()) { ROS_WARN_THROTTLE(1.0, "No valid points found"); return; } std::vector<DetectedLine> detected_lines; pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>); tree->setInputCloud(cloud); // 2. 执行欧几里得聚类 - 确保连续性 std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<PointT> ec; ec.setClusterTolerance(0.15); // 点间最大距离阈值(米) ec.setMinClusterSize(30); // 最小聚类点数 ec.setMaxClusterSize(10000); // 最大聚类点数 ec.setSearchMethod(tree); ec.setInputCloud(cloud); ec.extract(cluster_indices); ROS_INFO_THROTTLE(1.0, "Detected %zu point cloud clusters", cluster_indices.size()); std::vector<std::vector<uint8_t>> colors = { {255, 0, 0}, // 红 {0, 255, 0}, // 绿 {0, 0, 255}, // 蓝 {255, 255, 0}, // 黄 {0, 255, 255}, // 青 {255, 0, 255} // 紫 }; // 初始化点云颜色 for (auto& point : cloud->points) { point.r = 0; point.g = 255; point.b = 0; } // 3. 对每个聚类进行直线检测 for (size_t i = 0; i < cluster_indices.size(); i++) { const auto& cluster = cluster_indices[i]; // 创建当前聚类的点云 PointCloudT::Ptr cluster_cloud(new PointCloudT); for (const auto& idx : cluster.indices) { cluster_cloud->push_back((*cloud)[idx]); } // 为当前聚类的点着色 const auto& color = colors[i % colors.size()]; for (const auto& idx : cluster.indices) { cloud->points[idx].r = color[0]; cloud->points[idx].g = color[1]; cloud->points[idx].b = color[2]; } // 跳过点数过少的聚类 if (cluster_cloud->size() < 10) continue; pcl::SACSegmentation<PointT> seg; pcl::PointIndices::Ptr inliers(new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); seg.setOptimizeCoefficients(true); seg.setModelType(pcl::SACMODEL_LINE); seg.setMethodType(pcl::SAC_RANSAC); seg.setMaxIterations(1000); seg.setDistanceThreshold(0.05); // 点到直线的最大距离阈值 seg.setInputCloud(cluster_cloud); seg.segment(*inliers, *coefficients); if (inliers->indices.size() < 10) continue; DetectedLine line; float min_proj = std::numeric_limits<float>::max(); float max_proj = std::numeric_limits<float>::lowest(); Eigen::Vector3f direction(coefficients->values[3], coefficients->values[4], 0.0f); direction.normalize(); // 计算直线的起点和终点 for (const auto& idx : inliers->indices) { Eigen::Vector3f pt( cluster_cloud->points[idx].x, cluster_cloud->points[idx].y, 0.0f ); float proj = pt.dot(direction); if (proj < min_proj) { min_proj = proj; line.start.x = pt.x(); line.start.y = pt.y(); line.start.z = 0.0f; } if (proj > max_proj) { max_proj = proj; line.end.x = pt.x(); line.end.y = pt.y(); line.end.z = 0.0f; } } // 计算法线方向 line.normal.normal_x = -direction.y(); line.normal.normal_y = direction.x(); line.normal.normal_z = 0.0f; line.direction = direction; detected_lines.push_back(line); // 标记直线点 for (const auto& inlier_idx : inliers->indices) { if (inlier_idx >= 0 && inlier_idx < cluster.indices.size()) { int original_idx = cluster.indices[inlier_idx]; if (original_idx >= 0 && original_idx < cloud->size()) { point_data_map[original_idx].is_line_point = true; } } } } // 更新点云颜色 for (int i = 0; i < cloud->size(); i++) { if (point_data_map[i].is_line_point) { cloud->points[i].r = 255; cloud->points[i].g = 255; cloud->points[i].b = 0; } } sensor_msgs::PointCloud2 cloud_msg; pcl::toROSMsg(*cloud, cloud_msg); cloud_msg.header = msg->header; std::vector<DetectedLine> transformed_lines; geometry_msgs::Point left_front_tf_point_map ,left_back_tf_point_map ; geometry_msgs::Point right_front_tf_point_map , right_back_tf_point_map ; bool left_front_tf_valid = false , left_back_tf_valid = false; bool right_front_tf_valid = false , right_back_tf_valid = false; if (tfBuffer) { try { if (!tfBuffer->canTransform("base_footprint", cloud_msg.header.frame_id, cloud_msg.header.stamp, ros::Duration(0.1))) { ROS_WARN_THROTTLE(1.0, "TF transform not available"); return; } // 转换点云 sensor_msgs::PointCloud2 transformed_cloud; tfBuffer->transform(cloud_msg, transformed_cloud, "base_footprint"); transformed_cloud.header.stamp = ros::Time::now(); transformed_cloud.header.frame_id = "base_footprint"; pointcloud_pub.publish(transformed_cloud); // 转换检测到的直线 if (!detected_lines.empty()) { for (auto& line : detected_lines) { geometry_msgs::PointStamped laser_start, map_start; laser_start.header = msg->header; laser_start.point.x = line.start.x; laser_start.point.y = line.start.y; laser_start.point.z = 0.0f; tfBuffer->transform(laser_start, map_start, "base_footprint"); geometry_msgs::PointStamped laser_end, map_end; laser_end.header = msg->header; laser_end.point.x = line.end.x; laser_end.point.y = line.end.y; laser_end.point.z = 0.0f; tfBuffer->transform(laser_end, map_end, "base_footprint"); geometry_msgs::Vector3Stamped laser_normal, map_normal; laser_normal.header = msg->header; laser_normal.vector.x = line.normal.normal_x; laser_normal.vector.y = line.normal.normal_y; laser_normal.vector.z = 0.0f; tfBuffer->transform(laser_normal, map_normal, "base_footprint"); DetectedLine transformed_line; transformed_line.start.x = map_start.point.x; transformed_line.start.y = map_start.point.y; transformed_line.start.z = map_start.point.z; transformed_line.end.x = map_end.point.x; transformed_line.end.y = map_end.point.y; transformed_line.end.z = map_end.point.z; transformed_line.normal.normal_x = map_normal.vector.x; transformed_line.normal.normal_y = map_normal.vector.y; transformed_line.normal.normal_z = map_normal.vector.z; transformed_lines.push_back(transformed_line); } publishDetectedLines(transformed_lines, "base_footprint"); } // 获取TF参考点在base_footprint中的位置 try { geometry_msgs::PointStamped left_front_tf_laser, left_front_tf_map; left_front_tf_laser.header.frame_id = "laser_left_front"; left_front_tf_laser.header.stamp = ros::Time(0); left_front_tf_laser.point.x = left_front_ref.x; left_front_tf_laser.point.y = left_front_ref.y; left_front_tf_laser.point.z = left_front_ref.z; tfBuffer->transform(left_front_tf_laser, left_front_tf_map, "base_footprint"); left_front_tf_point_map = left_front_tf_map.point; left_front_tf_valid = true; geometry_msgs::PointStamped left_back_tf_laser, left_back_tf_map; left_back_tf_laser.header.frame_id = "laser_left_back"; left_back_tf_laser.header.stamp = ros::Time(0); left_back_tf_laser.point.x = left_back_ref.x; left_back_tf_laser.point.y = left_back_ref.y; left_back_tf_laser.point.z = left_back_ref.z; tfBuffer->transform(left_back_tf_laser, left_back_tf_map, "base_footprint"); left_back_tf_point_map = left_back_tf_map.point; left_back_tf_valid = true; geometry_msgs::PointStamped right_front_tf_laser, right_front_tf_map; right_front_tf_laser.header.frame_id = "laser_right_front"; right_front_tf_laser.header.stamp = ros::Time(0); right_front_tf_laser.point.x = right_front_ref.x; right_front_tf_laser.point.y = right_front_ref.y; right_front_tf_laser.point.z = right_front_ref.z; tfBuffer->transform(right_front_tf_laser, right_front_tf_map, "base_footprint"); right_front_tf_point_map = right_front_tf_map.point; right_front_tf_valid = true; geometry_msgs::PointStamped right_back_tf_laser, right_back_tf_map; right_back_tf_laser.header.frame_id = "laser_right_back"; right_back_tf_laser.header.stamp = ros::Time(0); right_back_tf_laser.point.x = right_back_ref.x; right_back_tf_laser.point.y = right_back_ref.y; right_back_tf_laser.point.z = right_back_ref.z; tfBuffer->transform(right_back_tf_laser, right_back_tf_map, "base_footprint"); right_back_tf_point_map = right_back_tf_map.point; right_back_tf_valid = true; publishTFPoints(left_front_tf_point_map, left_back_tf_point_map, right_front_tf_point_map, right_back_tf_point_map, "base_footprint"); } catch (tf2::TransformException& ex) { ROS_WARN_THROTTLE(1.0, "TF point transform error: %s", ex.what()); } } catch (tf2::TransformException& ex) { ROS_WARN_THROTTLE(1.0, "TF point transform error: %s", ex.what()); } } if ( ! transformed_lines.empty() && left_front_tf_valid && left_back_tf_valid && right_front_tf_valid && right_back_tf_valid) { std::vector<DetectedLine>left_walls; std::vector<DetectedLine>right_walls; for (const auto& wall : transformed_lines) { if (isLeftWall(wall)) { left_walls.push_back(wall); } else { right_walls.push_back(wall); } } double left_front_min_dist = std::numeric_limits<double>::max(); double left_back_min_dist = std::numeric_limits<double>::max(); geometry_msgs::Point left_front_foot , left_back_foot; geometry_msgs::Point left_front_wall_point, left_back_wall_point; for (const auto& wall : left_walls) { geometry_msgs::Point start_point, end_point; start_point.x = wall.start.x; start_point.y = wall.start.y; start_point.z = wall.start.z; end_point.x = wall.end.x; end_point.y = wall.end.y; end_point.z = wall.end.z; double dist_front = pointToLineDistance( left_front_tf_point_map, start_point, end_point); // 计算垂足点 geometry_msgs::Point foot_front = pointToLineProjection( left_front_tf_point_map, start_point, end_point ); if (dist_front < left_front_min_dist) { left_front_min_dist = dist_front; left_front_foot = foot_front; left_front_wall_point = foot_front; } double dist_back = pointToLineDistance( left_back_tf_point_map, start_point, end_point); // 计算垂足点 geometry_msgs::Point foot_back = pointToLineProjection( left_back_tf_point_map, start_point, end_point ); if (dist_back < left_back_min_dist) { left_back_min_dist = dist_back; left_back_foot = foot_back; left_back_wall_point = foot_back; } } //右墙 double right_front_min_dist = std::numeric_limits<double>::max(); double right_back_min_dist = std::numeric_limits<double>::max(); geometry_msgs::Point right_front_foot, right_back_foot; geometry_msgs::Point right_front_wall_point, right_back_wall_point; for (const auto& wall : right_walls) { geometry_msgs::Point start_point, end_point; start_point.x = wall.start.x; start_point.y = wall.start.y; start_point.z = wall.start.z; end_point.x = wall.end.x; end_point.y = wall.end.y; end_point.z = wall.end.z; // 计算右前参考点 double dist_front = pointToLineDistance( right_front_tf_point_map, start_point, end_point ); // 计算垂足点 geometry_msgs::Point foot_front = pointToLineProjection( right_front_tf_point_map, start_point, end_point ); if (dist_front < right_front_min_dist) { right_front_min_dist = dist_front; right_front_foot = foot_front; right_front_wall_point = foot_front; } // 计算右后参考点 double dist_back = pointToLineDistance( right_back_tf_point_map, start_point, end_point ); // 计算垂足点 geometry_msgs::Point foot_back = pointToLineProjection( right_back_tf_point_map, start_point, end_point ); if (dist_back < right_back_min_dist) { right_back_min_dist = dist_back; right_back_foot = foot_back; right_back_wall_point = foot_back; } } std_msgs::Float64 left_front_distance_msg, left_back_distance_msg , right_front_distance_msg , right_back_distance_msg; // 左前距离 double left_front_vertical_dist = (left_front_min_dist == std::numeric_limits<double>::max()) ? 10.0 : left_front_min_dist; left_front_distance_msg.data = left_front_vertical_dist; //left_front_nearest_distance_pub.publish(left_front_distance_msg); // 左后距离 double left_back_vertical_dist = (left_back_min_dist == std::numeric_limits<double>::max()) ? 10.0 : left_back_min_dist; left_back_distance_msg.data = left_back_vertical_dist; //left_back_nearest_distance_pub.publish(left_back_distance_msg); // 右前距离 double right_front_vertical_dist = (right_front_min_dist == std::numeric_limits<double>::max()) ? 10.0 : right_front_min_dist; right_front_distance_msg.data = right_front_vertical_dist; //right_front_nearest_distance_pub.publish(right_front_distance_msg); // 右后距离 double right_back_vertical_dist = (right_back_min_dist == std::numeric_limits<double>::max()) ? 10.0 : right_back_min_dist; right_back_distance_msg.data = right_back_vertical_dist; //right_back_nearest_distance_pub.publish(right_back_distance_msg); // 发布垂足点 publishFootPointMarker(left_front_foot, left_back_foot, right_front_foot, right_back_foot, "base_footprint"); // 发布距离信息 geometry_msgs::Point wall_mid_point; wall_mid_point.x = (left_front_wall_point.x + right_front_wall_point.x) / 2; wall_mid_point.y = (left_front_wall_point.y + right_front_wall_point.y) / 2; wall_mid_point.z = 0; float dist_min = min_four(left_front_vertical_dist,left_back_vertical_dist,right_front_vertical_dist,right_back_vertical_dist); if ( dist_min == left_front_vertical_dist ) { nearest_distance_pub.publish(left_front_distance_msg); } else if ( dist_min == left_back_vertical_dist ) { nearest_distance_pub.publish(left_back_distance_msg); } else if ( dist_min == right_front_vertical_dist ) { nearest_distance_pub.publish(right_front_distance_msg); } else { nearest_distance_pub.publish(right_back_distance_msg); } publishDistanceInfo("base_footprint", left_front_min_distance, left_back_min_distance, right_front_min_distance, right_back_min_distance, wall_mid_point); ROS_INFO_THROTTLE(1.0, "Left Front TF to wall: %.3f m, Left Back TF to wall: %.3f m , Right Front TF to wall: %.3f m, Right Bcak TF to wall: %.3f m", left_front_vertical_dist , left_back_vertical_dist ,right_front_vertical_dist ,right_back_vertical_dist ); } }”修改成你给我的样子,rviz观察到的四个tf坐标还是(0,0,0)
07-18
ros2中晃动imu,但是我的机器人模型不跟着晃动,而且里程计一开,里程计跑走,但是imu和机器人模型还在原地,以下是我的各个文件代码,情分析一下为什么:wheeltec_robot.cpp文件代码为: #include "turn_on_wheeltec_robot/wheeltec_robot.h" #include "turn_on_wheeltec_robot/Quaternion_Solution.h" #include "wheeltec_robot_msg/msg/data.hpp" sensor_msgs::msg::Imu Mpu6050;//Instantiate an IMU object //实例化IMU对象 using std::placeholders::_1; using namespace std; rclcpp::Node::SharedPtr node_handle = nullptr; //自动回充使用相关变量 bool check_AutoCharge_data = false; bool charge_set_state = false; /************************************** Date: January 28, 2021 Function: The main function, ROS initialization, creates the Robot_control object through the Turn_on_robot class and automatically calls the constructor initialization 功能: 主函数,ROS初始化,通过turn_on_robot类创建Robot_control对象并自动调用构造函数初始化 ***************************************/ int main(int argc, char** argv) { rclcpp::init(argc, argv); //ROS initializes and sets the node name //ROS初始化 并设置节点名称 turn_on_robot Robot_Control;//Instantiate an object //实例化一个对象 Robot_Control.Control();//Loop through data collection and publish the topic //循环执行数据采集和发布话题等操作 return 0; } /************************************** Date: January 28, 2021 Function: Data conversion function 功能: 数据转换函数 ***************************************/ short turn_on_robot::IMU_Trans(uint8_t Data_High,uint8_t Data_Low) { short transition_16; transition_16 = 0; transition_16 |= Data_High<<8; transition_16 |= Data_Low; return transition_16; } float turn_on_robot::Odom_Trans(uint8_t Data_High,uint8_t Data_Low) { float data_return; short transition_16; transition_16 = 0; transition_16 |= Data_High<<8; //Get the high 8 bits of data //获取数据的高8位 transition_16 |= Data_Low; //Get the lowest 8 bits of data //获取数据的低8位 data_return = (transition_16 / 1000)+(transition_16 % 1000)*0.001; // The speed unit is changed from mm/s to m/s //速度单位从mm/s转换为m/s return data_return; } /************************************** Date: January 28, 2021 Function: The speed topic subscription Callback function, according to the subscribed instructions through the serial port command control of the lower computer 功能: 速度话题订阅回调函数Callback,根据订阅的指令通过串口发指令控制下位机 ***************************************/ void turn_on_robot::Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux) { short transition; //intermediate variable //中间变量 Send_Data.tx[0]= 0x06; //frame head 0x7B //帧头0X7B //Send_Data.tx[1] = AutoRecharge; //set aside //预留位 Send_Data.tx[1] = FRAME_HEADER; Send_Data.tx[2] = 0; //set aside //预留位 //The target velocity of the X-axis of the robot //机器人x轴的目标线速度 transition=0; transition = twist_aux->linear.x*1000; //将浮点数放大一千倍,简化传输 (mm/s) Send_Data.tx[4] = transition; //取数据的低8位 Send_Data.tx[3] = transition>>8; //取数据的高8位 //The target velocity of the Y-axis of the robot //机器人y轴的目标线速度 transition=0; transition = twist_aux->linear.y*1000; Send_Data.tx[6] = transition; Send_Data.tx[5] = transition>>8; //The target angular velocity of the robot's Z axis //机器人z轴的目标角速度 transition=0; transition = twist_aux->angular.z*1000; //(1000*rad/s) Send_Data.tx[8] = transition; Send_Data.tx[7] = transition>>8; Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); //For the BCC check bits, see the Check_Sum function //BCC校验位,规则参见Check_Sum函数 Send_Data.tx[10]=0x0d; //frame tail 0x7D //帧尾0X7D Send_Data.tx[11]=0x0a; try { Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //Sends data to the downloader via serial port //通过串口向下位机发送数据 } catch (serial::IOException& e) { RCLCPP_ERROR(this->get_logger(),("Unable to send data through serial port")); //If sending data fails, an error message is printed //如果发送数据失败,打印错误信息 } } /************************************** Date: January 28, 2021 Function: Publish the IMU data topic 功能: 发布IMU数据话题 ***************************************/ void turn_on_robot::Publish_ImuSensor() { sensor_msgs::msg::Imu Imu_Data_Pub; //Instantiate IMU topic data //实例化IMU话题数据 Imu_Data_Pub.header.stamp = rclcpp::Node::now(); Imu_Data_Pub.header.frame_id = gyro_frame_id; //IMU corresponds to TF coordinates, which is required to use the robot_pose_ekf feature pack //IMU对应TF坐标,使用robot_pose_ekf功能包需要设置此项 Imu_Data_Pub.orientation.x = Mpu6050.orientation.x; //A quaternion represents a three-axis attitude //四元数表达三轴姿态 Imu_Data_Pub.orientation.y = Mpu6050.orientation.y; Imu_Data_Pub.orientation.z = Mpu6050.orientation.z; Imu_Data_Pub.orientation.w = Mpu6050.orientation.w; Imu_Data_Pub.orientation_covariance[0] = 1e6; //Three-axis attitude covariance matrix //三轴姿态协方差矩阵 Imu_Data_Pub.orientation_covariance[4] = 1e6; Imu_Data_Pub.orientation_covariance[8] = 1e-6; Imu_Data_Pub.angular_velocity.x = Mpu6050.angular_velocity.x; //Triaxial angular velocity //三轴角速度 Imu_Data_Pub.angular_velocity.y = Mpu6050.angular_velocity.y; Imu_Data_Pub.angular_velocity.z = Mpu6050.angular_velocity.z; Imu_Data_Pub.angular_velocity_covariance[0] = 1e6; //Triaxial angular velocity covariance matrix //三轴角速度协方差矩阵 Imu_Data_Pub.angular_velocity_covariance[4] = 1e6; Imu_Data_Pub.angular_velocity_covariance[8] = 1e-6; Imu_Data_Pub.linear_acceleration.x = Mpu6050.linear_acceleration.x; //Triaxial acceleration //三轴线性加速度 Imu_Data_Pub.linear_acceleration.y = Mpu6050.linear_acceleration.y; Imu_Data_Pub.linear_acceleration.z = Mpu6050.linear_acceleration.z; imu_publisher->publish(Imu_Data_Pub); //Pub IMU topic //发布IMU话题 } /************************************** Date: January 28, 2021 Function: Publish the odometer topic, Contains position, attitude, triaxial velocity, angular velocity about triaxial, TF parent-child coordinates, and covariance matrix 功能: 发布里程计话题,包含位置、姿态、三轴速度、绕三轴角速度、TF父子坐标、协方差矩阵 ***************************************/ void turn_on_robot::Publish_Odom() { //Convert the Z-axis rotation Angle into a quaternion for expression //把Z轴转角转换为四元数进行表达 tf2::Quaternion q; q.setRPY(0,0,Mpu6050_Data.imu_yaw); geometry_msgs::msg::Quaternion odom_quat=tf2::toMsg(q); nav_msgs::msg::Odometry odom; //Instance the odometer topic data //实例化里程计话题数据 odom.header.stamp = rclcpp::Node::now(); ; odom.header.frame_id = odom_frame_id; // Odometer TF parent coordinates //里程计TF父坐标 odom.pose.pose.position.x = Robot_Pos.X; //Position //位置 odom.pose.pose.position.y = Robot_Pos.Y; //odom.pose.pose.position.z = Robot_Pos.Z; odom.pose.pose.orientation = odom_quat; //Posture, Quaternion converted by Z-axis rotation //姿态,通过Z轴转角转换的四元数 odom.child_frame_id = robot_frame_id; // Odometer TF subcoordinates //里程计TF子坐标 odom.twist.twist.linear.x = Robot_Vel.X; //Speed in the X direction //X方向速度 //odom.twist.twist.linear.y = Robot_Vel.Y; //Speed in the Y direction //Y方向速度 odom.twist.twist.angular.z = Mpu6050.angular_velocity.z; //Angular velocity around the Z axis //绕Z轴角速度 //There are two types of this matrix, which are used when the robot is at rest and when it is moving.Extended Kalman Filtering officially provides 2 matrices for the robot_pose_ekf feature pack //这个矩阵有两种,分别在机器人静止和运动的时候使用。扩展卡尔曼滤波官方提供的2个矩阵,用于robot_pose_ekf功能包 //if(Robot_Vel.X== 0&&Robot_Vel.Y== 0&&Robot_Vel.Z== 0) if(Robot_Vel.X== 0) //If the velocity is zero, it means that the error of the encoder will be relatively small, and the data of the encoder will be considered more reliable //如果velocity是零,说明编码器的误差会比较小,认为编码器数据更可靠 memcpy(&odom.pose.covariance, odom_pose_covariance2, sizeof(odom_pose_covariance2)), memcpy(&odom.twist.covariance, odom_twist_covariance2, sizeof(odom_twist_covariance2)); else //If the velocity of the trolley is non-zero, considering the sliding error that may be brought by the encoder in motion, the data of IMU is considered to be more reliable //如果小车velocity非零,考虑到运动中编码器可能带来的滑动误差,认为imu的数据更可靠 memcpy(&odom.pose.covariance, odom_pose_covariance, sizeof(odom_pose_covariance)), memcpy(&odom.twist.covariance, odom_twist_covariance, sizeof(odom_twist_covariance)); odom_publisher->publish(odom); //Pub odometer topic //发布里程计话题 } /************************************** Date: January 28, 2021 Function: Publish voltage-related information 功能: 发布电压相关信息 ***************************************/ void turn_on_robot::Publish_Voltage() { std_msgs::msg::Float32 voltage_msgs; //Define the data type of the power supply voltage publishing topic //定义电源电压发布话题的数据类型 static float Count_Voltage_Pub=0; if(Count_Voltage_Pub++>10) { Count_Voltage_Pub=0; voltage_msgs.data = Power_voltage; //The power supply voltage is obtained //电源供电的电压获取 voltage_publisher->publish(voltage_msgs); //Post the power supply voltage topic unit: V, volt //发布电源电压话题单位:V、伏特 } } ////////// 回充发布与回调 //////// /************************************** Date: January 17, 2022 Function: Pub the topic whether the robot finds the infrared signal (charging station) 功能: 发布机器人是否寻找到红外信号(充电桩)的话题 ***************************************/ void turn_on_robot::Publish_RED() { std_msgs::msg::UInt8 msg; msg.data=Red; RED_publisher->publish(msg); } /************************************** Date: January 14, 2022 Function: Publish a topic about whether the robot is charging 功能: 发布机器人是否在充电的话题 ***************************************/ void turn_on_robot::Publish_Charging() { static bool last_charging; std_msgs::msg::Bool msg; msg.data=Charging; Charging_publisher->publish(msg); if(last_charging==false && Charging==true)cout<<GREEN<<"Robot is charging."<<endl<<RESET; if(last_charging==true && Charging==false)cout<<RED <<"Robot charging has disconnected."<<endl<<RESET; last_charging=Charging; } /************************************** Date: January 28, 2021 Function: Publish charging current information 功能: 发布充电电流信息 ***************************************/ void turn_on_robot::Publish_ChargingCurrent() { std_msgs::msg::Float32 msg; msg.data=Charging_Current; Charging_current_publisher->publish(msg); } /************************************** Date: March 1, 2022 Function: Infrared connection speed topic subscription Callback function, according to the subscription command through the serial port to set the infrared connection speed 功能: 红外对接速度话题订阅回调函数Callback,根据订阅的指令通过串口发指令设置红外对接速度 ***************************************/ void turn_on_robot::Red_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux) { short transition; //intermediate variable //中间变量 Send_Data.tx[0]=FRAME_HEADER; //frame head 0x7B //帧头0X7B Send_Data.tx[1] = 3; //Infrared docking speed setting flag bit = 3 //红外对接速度设置标志位=3 Send_Data.tx[2] = 0; //set aside //预留位 //The target velocity of the X-axis of the robot //机器人x轴的目标线速度 transition=0; transition = twist_aux->linear.x*1000; //将浮点数放大一千倍,简化传输 Send_Data.tx[4] = transition; //取数据的低8位 Send_Data.tx[3] = transition>>8; //取数据的高8位 //The target velocity of the Y-axis of the robot //机器人y轴的目标线速度 transition=0; transition = twist_aux->linear.y*1000; Send_Data.tx[6] = transition; Send_Data.tx[5] = transition>>8; //The target angular velocity of the robot's Z axis //机器人z轴的目标角速度 transition=0; transition = twist_aux->angular.z*1000; Send_Data.tx[8] = transition; Send_Data.tx[7] = transition>>8; Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); //BCC check //BCC校验 Send_Data.tx[10]=FRAME_TAIL; //frame tail 0x7D //帧尾0X7D try { Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //Sends data to the downloader via serial port //通过串口向下位机发送数据 } catch (serial::IOException& e) { RCLCPP_ERROR(this->get_logger(),("Unable to send data through serial port")); //If sending data fails, an error message is printed //如果发送数据失败,打印错误信息 } } /************************************** Date: January 14, 2022 Function: Subscription robot recharge flag bit topic, used to tell the lower machine speed command is normal command or recharge command 功能: 订阅机器人是否回充标志位话题,用于告诉下位机速度命令是正常命令还是回充命令 ***************************************/ void turn_on_robot::Recharge_Flag_Callback(const std_msgs::msg::Int8::SharedPtr Recharge_Flag) { AutoRecharge=Recharge_Flag->data; } //服务 void turn_on_robot::Set_Charge_Callback(const shared_ptr<turtlesim::srv::Spawn::Request> req,shared_ptr<turtlesim::srv::Spawn::Response> res) { Send_Data.tx[0]=FRAME_HEADER; //frame head 0x7B //֡ͷ0X7B if(round(req->x)==1) Send_Data.tx[1] = 1; else if(round(req->x)==2) Send_Data.tx[1] = 2; else if(round(req->x)==0) Send_Data.tx[1] = 0,AutoRecharge=0; Send_Data.tx[2] = 0; Send_Data.tx[3] = 0; Send_Data.tx[4] = 0; Send_Data.tx[5] = 0; Send_Data.tx[6] = 0; Send_Data.tx[7] = 0; Send_Data.tx[8] = 0; Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); //For the BCC check bits, see the Check_Sum function //BCCУ��λ������μ�Check_Sum���� Send_Data.tx[10]=FRAME_TAIL; //frame tail 0x7D //֡β0X7D try { Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //Sends data to the downloader via serial port //ͨ����������λ���������� } catch (serial::IOException& e) { res->name = "false"; } if( Send_Data.tx[1]==0 ) { if(charge_set_state==0) AutoRecharge=0,res->name = "true"; else res->name = "false"; } else { if(charge_set_state==1) res->name = "true"; else res->name = "false"; } return; } ////////// 回充发布与回调 //////// /************************************** Date: January 28, 2021 Function: Serial port communication check function, packet n has a byte, the NTH -1 byte is the check bit, the NTH byte bit frame end.Bit XOR results from byte 1 to byte n-2 are compared with byte n-1, which is a BCC check Input parameter: Count_Number: Check the first few bytes of the packet 功能: 串口通讯校验函数,数据包n有个字节,第n-1个字节为校验位,第n个字节位帧尾。第1个字节到第n-2个字节数据按位异或的结果与第n-1个字节对比,即为BCC校验 输入参数: Count_Number:数据包前几个字节加入校验 mode:对发送数据还是接收数据进行校验 ***************************************/ unsigned char turn_on_robot::Check_Sum(unsigned char Count_Number,unsigned char mode) { unsigned char check_sum=0,k; if(mode==0) //Receive data mode //接收数据模式 { for(k=0;k<Count_Number;k++) { check_sum=check_sum^Receive_Data.rx[k]; //By bit or by bit //按位异或 } } if(mode==1) //Send data mode //发送数据模式 { for(k=1;k<Count_Number;k++) { check_sum=check_sum^Send_Data.tx[k]; //By bit or by bit //按位异或 } } return check_sum; //Returns the bitwise XOR result //返回按位异或结果 } //自动回充专用校验位 unsigned char turn_on_robot::Check_Sum_AutoCharge(unsigned char Count_Number,unsigned char mode) { unsigned char check_sum=0,k; if(mode==0) //Receive data mode //接收数据模式 { for(k=0;k<Count_Number;k++) { check_sum=check_sum^Receive_AutoCharge_Data.rx[k]; //By bit or by bit //按位异或 } } return check_sum; } /************************************** Date: November 18, 2021 Function: Read and verify the data sent by the lower computer frame by frame through the serial port, and then convert the data into international units 功能: 通过串口读取并逐帧校验下位机发送过来的数据,然后数据转换为国际单位 ***************************************/ bool turn_on_robot::Get_Sensor_Data_New() { short transition_16=0; //Intermediate variable //中间变量 //uint8_t i=0; uint8_t check=0,check2=0, error=1,error2=1,Receive_Data_Pr[1]; //Temporary variable to save the data of the lower machine //临时变量,保存下位机数据 static int count,count2; //Static variable for counting //静态变量,用于计数 Stm32_Serial.read(Receive_Data_Pr,sizeof(Receive_Data_Pr)); //Read the data sent by the lower computer through the serial port //通过串口读取下位机发送过来的数据 /*//View the received raw data directly and debug it for use//直接查看接收到的原始数据,调试使用 ROS_INFO("%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x", Receive_Data_Pr[0],Receive_Data_Pr[1],Receive_Data_Pr[2],Receive_Data_Pr[3],Receive_Data_Pr[4],Receive_Data_Pr[5],Receive_Data_Pr[6],Receive_Data_Pr[7], Receive_Data_Pr[8],Receive_Data_Pr[9],Receive_Data_Pr[10],Receive_Data_Pr[11],Receive_Data_Pr[12],Receive_Data_Pr[13],Receive_Data_Pr[14],Receive_Data_Pr[15], Receive_Data_Pr[16],Receive_Data_Pr[17],Receive_Data_Pr[18],Receive_Data_Pr[19],Receive_Data_Pr[20],Receive_Data_Pr[21],Receive_Data_Pr[22],Receive_Data_Pr[23]); */ Receive_Data.rx[count] = Receive_Data_Pr[0]; //Fill the array with serial data //串口数据填入数组 Receive_AutoCharge_Data.rx[count2] = Receive_Data_Pr[0]; Receive_Data.Frame_Header = Receive_Data.rx[0]; //The first part of the data is the frame header 0X7B //数据的第一位是帧头0X7B Receive_Data.Frame_Tail = Receive_Data.rx[23]; //The last bit of data is frame tail 0X7D //数据的最后一位是帧尾0X7D //接收到自动回充数据的帧头、上一个数据是24字节的帧尾,表明自动回充数据开始到来 if((Receive_Data_Pr[0] == AutoCharge_HEADER )||count2>0) count2++; else count2=0; if(Receive_Data_Pr[0] == FRAME_HEADER || count>0) //Ensure that the first data in the array is FRAME_HEADER //确保数组第一个数据为FRAME_HEADER count++; else count=0; //自动回充数据处理 if(count2 == AutoCharge_DATA_SIZE) { count2=0; if(Receive_AutoCharge_Data.rx[AutoCharge_DATA_SIZE-1]==AutoCharge_TAIL) //确认帧尾 { check2 = Check_Sum_AutoCharge(6,0);//校验位计算 if(check2 == Receive_AutoCharge_Data.rx[AutoCharge_DATA_SIZE-2]) //校验正确 { error2=0; } if(error2 == 0) //校验正确开始赋值 { transition_16 = 0; transition_16 |= Receive_AutoCharge_Data.rx[1]<<8; transition_16 |= Receive_AutoCharge_Data.rx[2]; Charging_Current = transition_16/1000+(transition_16 % 1000)*0.001; //充电电流 Red = Receive_AutoCharge_Data.rx[3]; //红外接受状态 Charging = Receive_AutoCharge_Data.rx[4];//小车充电状态 charge_set_state = Receive_AutoCharge_Data.rx[5]; check_AutoCharge_data = true; //数据成功接收标志位 } } } if(count == 24) //Verify the length of the packet //验证数据包的长度 { count=0; //Prepare for the serial port data to be refill into the array //为串口数据重新填入数组做准备 if(Receive_Data.Frame_Tail == FRAME_TAIL) //Verify the frame tail of the packet //验证数据包的帧尾 { check=Check_Sum(22,READ_DATA_CHECK); //BCC check passes or two packets are interlaced //BCC校验通过或者两组数据包交错 if(check == Receive_Data.rx[22]) { error=0; //XOR bit check successful //异或位校验成功 } if(error == 0) { /*//Check receive_data.rx for debugging use //查看Receive_Data.rx,调试使用 ROS_INFO("%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x", Receive_Data.rx[0],Receive_Data.rx[1],Receive_Data.rx[2],Receive_Data.rx[3],Receive_Data.rx[4],Receive_Data.rx[5],Receive_Data.rx[6],Receive_Data.rx[7], Receive_Data.rx[8],Receive_Data.rx[9],Receive_Data.rx[10],Receive_Data.rx[11],Receive_Data.rx[12],Receive_Data.rx[13],Receive_Data.rx[14],Receive_Data.rx[15], Receive_Data.rx[16],Receive_Data.rx[17],Receive_Data.rx[18],Receive_Data.rx[19],Receive_Data.rx[20],Receive_Data.rx[21],Receive_Data.rx[22],Receive_Data.rx[23]); */ Receive_Data.Flag_Stop=Receive_Data.rx[1]; //set aside //预留位 Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]); //Get the speed of the moving chassis in the X direction //获取运动底盘X方向速度 //Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]); //Get the speed of the moving chassis in the Y direction, The Y speed is only valid in the omnidirectional mobile robot chassis //获取运动底盘Y方向速度,Y速度仅在全向移动机器人底盘有效 //Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]); //Get the speed of the moving chassis in the Z direction //获取运动底盘Z方向速度 //MPU6050 stands for IMU only and does not refer to a specific model. It can be either MPU6050 or MPU9250 //Mpu6050仅代表IMU,不指代特定型号,既可以是MPU6050也可以是MPU9250 Mpu6050_Data.imu_rol = IMU_Trans(Receive_Data.rx[4],Receive_Data.rx[5]) * 0.01 * 3.14159/180; Mpu6050_Data.imu_yaw = IMU_Trans(Receive_Data.rx[6],Receive_Data.rx[7]) * 0.01 * 3.14159/180; Mpu6050_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8],Receive_Data.rx[9]); //Get the X-axis acceleration of the IMU //获取IMU的X轴加速度 Mpu6050_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10],Receive_Data.rx[11]); //Get the Y-axis acceleration of the IMU //获取IMU的Y轴加速度 Mpu6050_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12],Receive_Data.rx[13]); //Get the Z-axis acceleration of the IMU //获取IMU的Z轴加速度 Mpu6050_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14],Receive_Data.rx[15]); //Get the X-axis angular velocity of the IMU //获取IMU的X轴角速度 Mpu6050_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16],Receive_Data.rx[17]); //Get the Y-axis angular velocity of the IMU //获取IMU的Y轴角速度 Mpu6050_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18],Receive_Data.rx[19]); //Get the Z-axis angular velocity of the IMU //获取IMU的Z轴角速度 //Linear acceleration unit conversion is related to the range of IMU initialization of STM32, where the range is ±2g=19.6m/s^2 //线性加速度单位转化,和STM32的IMU初始化的时候的量程有关,这里量程±2g=19.6m/s^2 Mpu6050.linear_acceleration.x = Mpu6050_Data.accele_x_data * 0.01 * 9.8; Mpu6050.linear_acceleration.y = Mpu6050_Data.accele_y_data * 0.01 * 9.8; Mpu6050.linear_acceleration.z = Mpu6050_Data.accele_z_data * 0.01 * 9.8; //The gyroscope unit conversion is related to the range of STM32's IMU when initialized. Here, the range of IMU's gyroscope is ±500°/s //Because the robot generally has a slow Z-axis speed, reducing the range can improve the accuracy //陀螺仪单位转化,和STM32的IMU初始化的时候的量程有关,这里IMU的陀螺仪的量程是±500°/s //因为机器人一般Z轴速度不快,降低量程可以提高精度 Mpu6050.angular_velocity.x = Mpu6050_Data.gyros_x_data * 0.01 *3.14159/180; Mpu6050.angular_velocity.y = Mpu6050_Data.gyros_y_data * 0.01 *3.14159/180; Mpu6050.angular_velocity.z = Mpu6050_Data.gyros_z_data * 0.01 *3.14159/180; //Get the battery voltage //获取电池电压 transition_16 = 0; transition_16 |= Receive_Data.rx[20]<<8; transition_16 |= Receive_Data.rx[21]; Power_voltage = transition_16/1000+(transition_16 % 1000)*0.001; //Unit conversion millivolt(mv)->volt(v) //单位转换毫伏(mv)->伏(v) return true; } } } return false; } /************************************** Date: January 28, 2021 Function: Loop access to the lower computer data and issue topics 功能: 循环获取下位机数据与发布话题 ***************************************/ void turn_on_robot::Control() { //_Last_Time = ros::Time::now(); _Last_Time = rclcpp::Node::now(); while(rclcpp::ok()) { try { //_Now = ros::Time::now(); _Now = rclcpp::Node::now(); Sampling_Time = (_Now - _Last_Time).seconds(); //Retrieves time interval, which is used to integrate velocity to obtain displacement (mileage) //获取时间间隔,用于积分速度获得位移(里程) if (true == Get_Sensor_Data_New()) //The serial port reads and verifies the data sent by the lower computer, and then the data is converted to international units //通过串口读取并校验下位机发送过来的数据,然后数据转换为国际单位 { //Robot_Pos.X+=(Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time; //Calculate the displacement in the X direction, unit: m //计算X方向的位移,单位:m //Robot_Pos.Y+=(Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time; //Calculate the displacement in the Y direction, unit: m //计算Y方向的位移,单位:m //Robot_Pos.Z+=Robot_Vel.Z * Sampling_Time; //The angular displacement about the Z axis, in rad //绕Z轴的角位移,单位:rad Robot_Pos.X+=(Robot_Vel.X * cos(Mpu6050_Data.imu_yaw)) * Sampling_Time; //Calculate the displacement in the X direction, unit: m //计算X方向的位移,单位:m Robot_Pos.Y+=(Robot_Vel.X * sin(Mpu6050_Data.imu_yaw)) * Sampling_Time; //Calculate the displacement in the Y direction, unit: m //计算Y方向的位移,单位:m //Robot_Pos.Z = 0 ; //Calculate the three-axis attitude from the IMU with the angular velocity around the three-axis and the three-axis acceleration //通过IMU绕三轴角速度与三轴加速度计算三轴姿态 Quaternion_Solution(Mpu6050.angular_velocity.x, Mpu6050.angular_velocity.y, Mpu6050.angular_velocity.z,\ Mpu6050.linear_acceleration.x, Mpu6050.linear_acceleration.y, Mpu6050.linear_acceleration.z); Publish_Odom(); //Pub the speedometer topic //发布里程计话题 Publish_ImuSensor(); //Pub the IMU topic //发布IMU话题 Publish_Voltage(); //Pub the topic of power supply voltage //发布电源电压话题 _Last_Time = _Now; //Record the time and use it to calculate the time interval //记录时间,用于计算时间间隔 } //自动回充数据话题 if(check_AutoCharge_data) { Publish_Charging(); //Pub a topic about whether the robot is charging //发布机器人是否在充电的话题 Publish_RED(); //Pub the topic whether the robot finds the infrared signal (charging station) //发布机器人是否寻找到红外信号(充电桩)的话题 Publish_ChargingCurrent(); //Pub the charging current topic //发布充电电流话题 check_AutoCharge_data = false; } rclcpp::spin_some(this->get_node_base_interface()); //The loop waits for the callback function //循环等待回调函数 } catch (const rclcpp::exceptions::RCLError & e ) { RCLCPP_ERROR(this->get_logger(),"unexpectedly failed whith %s",e.what()); } } } /************************************** Date: January 28, 2021 Function: Constructor, executed only once, for initialization 功能: 构造函数, 只执行一次,用于初始化 ***************************************/ turn_on_robot::turn_on_robot():rclcpp::Node ("wheeltec_robot") { Sampling_Time=0; Power_voltage=0; //Clear the data //清空数据 memset(&Robot_Pos, 0, sizeof(Robot_Pos)); memset(&Robot_Vel, 0, sizeof(Robot_Vel)); memset(&Receive_Data, 0, sizeof(Receive_Data)); memset(&Send_Data, 0, sizeof(Send_Data)); memset(&Mpu6050_Data, 0, sizeof(Mpu6050_Data)); //ros::NodeHandle private_nh("~"); //Create a node handle //创建节点句柄 //The private_nh.param() entry parameter corresponds to the initial value of the name of the parameter variable on the parameter server //private_nh.param()入口参数分别对应:参数服务器上的名称 参数变量名 初始值 this->declare_parameter<int>("serial_baud_rate",115200); this->declare_parameter<std::string>("usart_port_name", "/dev/ttyACM0"); this->declare_parameter<std::string>("odom_frame_id", "odom_combined"); this->declare_parameter<std::string>("robot_frame_id", "base_footprint"); this->declare_parameter<std::string>("gyro_frame_id", "gyro_link"); this->get_parameter("serial_baud_rate", serial_baud_rate);//Communicate baud rate 115200 to the lower machine //和下位机通信波特率115200 this->get_parameter("usart_port_name", usart_port_name);//Fixed serial port number //固定串口号 this->get_parameter("odom_frame_id", odom_frame_id);//The odometer topic corresponds to the parent TF coordinate //里程计话题对应父TF坐标 this->get_parameter("robot_frame_id", robot_frame_id);//The odometer topic corresponds to sub-TF coordinates //里程计话题对应子TF坐标 this->get_parameter("gyro_frame_id", gyro_frame_id);//IMU topics correspond to TF coordinates //IMU话题对应TF坐标 odom_publisher = create_publisher<nav_msgs::msg::Odometry>("odom", 2);//Create the odometer topic publisher //创建里程计话题发布者 imu_publisher = create_publisher<sensor_msgs::msg::Imu>("imu/data_raw", 2); //Create an IMU topic publisher //创建IMU话题发布者 voltage_publisher = create_publisher<std_msgs::msg::Float32>("PowerVoltage", 1);//Create a battery-voltage topic publisher //创建电池电压话题发布者 //回充发布者 Charging_publisher = create_publisher<std_msgs::msg::Bool>("robot_charging_flag", 10); Charging_current_publisher = create_publisher<std_msgs::msg::Float32>("robot_charging_current", 10); RED_publisher = create_publisher<std_msgs::msg::UInt8>("robot_red_flag", 10); //回充订阅者 Red_Vel_Sub = create_subscription<geometry_msgs::msg::Twist>( "red_vel", 10, std::bind(&turn_on_robot::Red_Vel_Callback, this, std::placeholders::_1)); Recharge_Flag_Sub = create_subscription<std_msgs::msg::Int8>( "robot_recharge_flag", 10, std::bind(&turn_on_robot::Recharge_Flag_Callback, this,std::placeholders::_1)); //回充服务提供 SetCharge_Service=this->create_service<turtlesim::srv::Spawn>\ ("/set_charge",std::bind(&turn_on_robot::Set_Charge_Callback,this, std::placeholders::_1 ,std::placeholders::_2)); //Set the velocity control command callback function //速度控制命令订阅回调函数设置 Cmd_Vel_Sub = create_subscription<geometry_msgs::msg::Twist>( "cmd_vel", 2, std::bind(&turn_on_robot::Cmd_Vel_Callback, this, _1)); RCLCPP_INFO(this->get_logger(),"wheeltec_robot Data ready"); //Prompt message //提示信息 try { //Attempts to initialize and open the serial port //尝试初始化与开启串口 Stm32_Serial.setPort(usart_port_name); //Select the serial port number to enable //选择要开启的串口号 Stm32_Serial.setBaudrate(serial_baud_rate); //Set the baud rate //设置波特率 serial::Timeout _time = serial::Timeout::simpleTimeout(2000); //Timeout //超时等待 Stm32_Serial.setTimeout(_time); Stm32_Serial.open(); //Open the serial port //开启串口 } catch (serial::IOException& e) { RCLCPP_ERROR(this->get_logger(),"wheeltec_robot can not open serial port,Please check the serial port cable! "); //If opening the serial port fails, an error message is printed //如果开启串口失败,打印错误信息 } if(Stm32_Serial.isOpen()) { RCLCPP_INFO(this->get_logger(),"wheeltec_robot serial port opened"); //Serial port opened successfully //串口开启成功提示 } } /************************************** Date: January 28, 2021 Function: Destructor, executed only once and called by the system when an object ends its life cycle 功能: 析构函数,只执行一次,当对象结束其生命周期时系统会调用这个函数 ***************************************/ turn_on_robot::~turn_on_robot() { //Sends the stop motion command to the lower machine before the turn_on_robot object ends //对象turn_on_robot结束前向下位机发送停止运动命令 Send_Data.tx[0]=0x06; Send_Data.tx[1] = FRAME_HEADER; Send_Data.tx[2] = 0; //The target velocity of the X-axis of the robot //机器人X轴的目标线速度 Send_Data.tx[4] = 0; Send_Data.tx[3] = 0; //The target velocity of the Y-axis of the robot //机器人Y轴的目标线速度 Send_Data.tx[6] = 0; Send_Data.tx[5] = 0; //The target velocity of the Z-axis of the robot //机器人Z轴的目标角速度 Send_Data.tx[8] = 0; Send_Data.tx[7] = 0; Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); //Check the bits for the Check_Sum function //校验位,规则参见Check_Sum函数 Send_Data.tx[10]=0x0d; Send_Data.tx[11]=0x0a; try { Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //Send data to the serial port //向串口发数据 } catch (serial::IOException& e) { RCLCPP_ERROR(this->get_logger(),"Unable to send data through serial port"); //If sending data fails, an error message is printed //如果发送数据失败,打印错误信息 } Stm32_Serial.close(); //Close the serial port //关闭串口 RCLCPP_INFO(this->get_logger(),"Shutting down"); //Prompt message //提示信息 } turn_on_wheeltec_robot.launch.py文件代码为: import os from pathlib import Path import launch from launch.actions import SetEnvironmentVariable from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import (DeclareLaunchArgument, GroupAction, IncludeLaunchDescription, SetEnvironmentVariable) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import PushRosNamespace import launch_ros.actions from launch.conditions import IfCondition from launch.conditions import UnlessCondition def generate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory('turn_on_wheeltec_robot') launch_dir = os.path.join(bringup_dir, 'launch') ekf_config = Path(get_package_share_directory('turn_on_wheeltec_robot'), 'config', 'ekf.yaml') ekf_carto_config = Path(get_package_share_directory('turn_on_wheeltec_robot'), 'config', 'ekf_carto.yaml') imu_config = Path(get_package_share_directory('turn_on_wheeltec_robot'), 'config', 'imu.yaml') carto_slam = LaunchConfiguration('carto_slam', default='false') carto_slam_dec = DeclareLaunchArgument('carto_slam',default_value='false') wheeltec_robot = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, 'base_serial.launch.py')), ) robot_ekf = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, 'wheeltec_ekf.launch.py')), launch_arguments={'carto_slam':carto_slam}.items(), ) base_to_link = launch_ros.actions.Node( package='tf2_ros', executable='static_transform_publisher', name='base_to_link', arguments=['0', '0', '0','0', '0','0','base_footprint','base_link'], ) base_to_gyro = launch_ros.actions.Node( package='tf2_ros', executable='static_transform_publisher', name='base_to_gyro', arguments=['0', '0', '0','0', '0','0','base_link','gyro_link'], ) imu_filter_node = launch_ros.actions.Node( package='imu_filter_madgwick', executable='imu_filter_madgwick_node', parameters=[imu_config] ) # joint_state_publisher_node = launch_ros.actions.Node( # package='joint_state_publisher', # executable='joint_state_publisher', # name='joint_state_publisher', # ) #select a robot model,the default model is mini_mec #minibot.launch.py contains commonly used robot models #launch_arguments choices:mini_mec/mini_akm/mini_tank/mini_4wd/mini_diff/mini_omni/brushless_senior_diff #!!!At the same time, you need to modify ld.add_action(minibot_type) and #ld.add_action(flagship_type) minibot_type = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, 'robot_mode_description_minibot.launch.py')), launch_arguments={'bike_robot': 'true'}.items(), ) #robot_mode_description.launch.py contains flagship products, usually larger chassis robots #launch_arguments choices: #senior_akm/top_akm_bs/top_akm_dl #senior_mec_bs/senior_mec_dl/top_mec_bs/top_mec_dl/ mec_EightDrive_robot/flagship_mec_bs_robot/flagship_mec_dl_robot�� #senior_omni/top_omni #senior_4wd_bs_robot/senior_4wd_dl_robot/flagship_4wd_bs_robot/flagship_4wd_dl_robot/top_4wd_bs_robot/top_4wd_dl_robot #senior_diff_robot/four_wheel_diff_bs/four_wheel_diff_dl/flagship_four_wheel_diff_bs_robot/flagship_four_wheel_diff_dl_robot #!!!At the same time, you need to modify ld.add_action(flagship_type) and #ld.add_action(minibot_type) flagship_type = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, 'robot_mode_description.launch.py')), launch_arguments={'senior_akm': 'true'}.items(), ) ld = LaunchDescription() ld.add_action(minibot_type) #ld.add_action(flagship_type) ld.add_action(carto_slam_dec) ld.add_action(wheeltec_robot) ld.add_action(base_to_link) ld.add_action(base_to_gyro) # ld.add_action(joint_state_publisher_node) ld.add_action(imu_filter_node) ld.add_action(robot_ekf) return ld base_serial.launch.py文件代码为: from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch.conditions import IfCondition from launch.conditions import UnlessCondition import launch_ros.actions #def launch(launch_descriptor, argv): def generate_launch_description(): return LaunchDescription([ launch_ros.actions.Node( package='turn_on_wheeltec_robot', executable='wheeltec_robot_node', output='screen', parameters=[{'usart_port_name': '/dev/ttyACM0', 'serial_baud_rate':115200, 'robot_frame_id': 'base_footprint', 'odom_frame_id': 'odom_combined', 'cmd_vel': 'cmd_vel',}], ) ]) robot_mode_description_minibot.launch.py文件代码为: import os from ament_index_python.packages import get_package_share_directory import launch_ros.actions from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import LoadComposableNodes from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode from nav2_common.launch import RewrittenYaml def generate_robot_node(robot_urdf,child): return launch_ros.actions.Node( package='robot_state_publisher', executable='robot_state_publisher', name=f'robot_state_publisher_{child}', arguments=[os.path.join(get_package_share_directory('wheeltec_robot_urdf'), 'urdf', robot_urdf)], ) def generate_static_transform_publisher_node(translation, rotation, parent, child): return launch_ros.actions.Node( package='tf2_ros', executable='static_transform_publisher', name=f'base_to_{child}', arguments=[translation[0], translation[1], translation[2], rotation[0], rotation[1], rotation[2], parent, child], ) def generate_launch_description(): bike_robot = LaunchConfiguration('bike_robot', default='false') bike_robot_ = GroupAction( condition=IfCondition(bike_robot), actions=[ generate_robot_node('xuan_bike_robot.urdf','bike_robot'), generate_static_transform_publisher_node(['0.048', '0', '0.18'], ['0', '0', '0'], 'base_link', 'laser'), generate_static_transform_publisher_node(['0.048', '0', '0.18'], ['0', '0', '0'], 'base_link', 'camera_link'), ]) # Create the launch description and populate ld = LaunchDescription() # Set environment variables # Declare the launch options #ld.add_action(declare_use_composition_cmd) # Add the actions to launch all of the localiztion nodes ld.add_action(mini_mec_) ld.add_action(mini_akm_) ld.add_action(mini_tank_) ld.add_action(mini_4wd_) ld.add_action(mini_diff_) ld.add_action(brushless_senior_diff_) ld.add_action(bike_robot_) return ld xuan_bike_robot.urdf文件代码为: <?xml version="1.0"?> <robot name="wheeltec_robot"> <link name="base_link"> <visual> <geometry> <mesh filename="file:///home/deepbot/ros-relate/wheeltec_ros2/src/wheeltec_robot_urdf/wheeltec_robot_urdf/meshes/Xuan-release.stl"scale="0.001 0.001 0.001"/> </geometry> <origin xyz="0 0 0" rpy="0 0 0"/> <material name="blue"> <color rgba="0 0 0.8 0.5"/> </material> </visual> </link> </robot>
05-14
我们具备ar_track_alvar的软件包,这个是arm_controller.cpp/* * Software License Agreement (BSD License) * Copyright (c) 2020, reinovo, Inc. * All rights reserved. * Author: LN <825255961@qq.com> */ #include <string> #include <vector> #include <sstream> #include <cmath> #include <cstdlib>//string转化为double #include <iomanip>//保留有效小数 #include "unistd.h" #include <geometry_msgs/Twist.h> #include <tf/transform_broadcaster.h> #include <ros/ros.h> #include <serial/serial.h> #include <std_msgs/String.h> #include <std_srvs/SetBool.h> #include "arm_controller/move.h" #include "arm_controller/control.h" using namespace std; class ARM_CONTROL { public: ARM_CONTROL(); // virtual ~ARM_CONTROL(); //获取位置 bool get_pos(); //移动到目标点 bool goto_pos(float gx,float gy,float gz); //goto回调函数 bool goto_position_deal(arm_controller::move::Request &req, arm_controller::move::Response &res); //吸盘控制 bool pump_callback(std_srvs::SetBool::Request &req,std_srvs::SetBool::Response &res); //解锁电机 bool unlcok_callback(std_srvs::SetBool::Request &req,std_srvs::SetBool::Response &res); //手臂定向移动 void cmd_callback(const geometry_msgs::Twist& msg); private: ros::NodeHandle nh; std::string read_data; serial::Serial _serial; // serial object arm_controller::control pos; ros::Subscriber sub; ros::Publisher pub; ros::ServiceServer goto_position_server,pick_server,unlock_server; bool unlcok_aign; //tf广播器 tf::TransformBroadcaster robot_broadcaster; }; void ARM_CONTROL::cmd_callback(const geometry_msgs::Twist& msg) { std::string Gcode = ""; //float转字符 char x[10]; char y[10]; char z[10]; sprintf(x, "%.2f", msg.linear.x); sprintf(y, "%.2f", msg.linear.y); sprintf(z, "%.2f", msg.linear.z); Gcode = (std::string)"G2204 X" + x + " Y" + y + " Z" + z + " F1000" "\n"; ROS_INFO("%s", Gcode.c_str()); //将移动数据发送到串口 _serial.write(Gcode.c_str()); } //获取位姿 bool ARM_CONTROL::get_pos() { // //向串口发送P2220获取位姿 std::string Gcode = "", data=""; // Gcode = (std::string)"P2220" + "\r\n"; // _serial.write(Gcode.c_str()); //读取串口返回消息 data = _serial.read(_serial.available()); //std::cout << data << endl; //分割返回数据 std::vector<std::string> v; std::string::size_type pos1, pos2, pos3; pos1 = data.find("X"); pos2 = data.find("Y"); pos3 = data.find("Z"); v.push_back(data.substr(pos1+1,pos2-pos1)); //X v.push_back(data.substr(pos2+1,pos3-pos2)); //Y v.push_back(data.substr(pos3+1,data.length()-pos3-1)); //Z //赋值 pos.position.x = std::atof(v[0].c_str()); pos.position.y = std::atof(v[1].c_str()); pos.position.z = std::atof(v[2].c_str()); if (pos.position.x < 0.1 && pos.position.y < 0.1 && pos.position.z < 0.1) return false; else{ //发布话题 arm_controller/position_info pub.publish(pos); geometry_msgs::Quaternion robot_quat = tf::createQuaternionMsgFromYaw(atan2(pos.position.y,pos.position.x)); geometry_msgs::TransformStamped robot_trans; robot_trans.header.stamp = ros::Time::now(); robot_trans.header.frame_id = "robot"; robot_trans.child_frame_id = "end"; robot_trans.transform.translation.x = pos.position.x/1000; robot_trans.transform.translation.y = pos.position.y/1000; robot_trans.transform.translation.z = pos.position.z/1000; robot_trans.transform.rotation = robot_quat; robot_broadcaster.sendTransform(robot_trans); return true; } } //goto pos bool ARM_CONTROL::goto_pos(float gx,float gy,float gz) { std::string Gcode = ""; string result; ros::Rate loop_rate(20); _serial.read(_serial.available()); //float转字符 char x[10]; char y[10]; char z[10]; sprintf(x, "%.2f", gx); sprintf(y, "%.2f", gy); sprintf(z, "%.2f", gz); Gcode = (std::string)"M2222 X" + x + " Y" + y + " Z" + z + " P0\n"; ROS_INFO("发送数据 : %s", Gcode.c_str()); //将移动数据发送到串口 _serial.write(Gcode.c_str()); //延时100ms usleep(50*1000); //读取返回 result = _serial.read(_serial.available()); cout << result << endl; if (result.find("V0") < 100){ //如果该点不在工作范围内 return false; }else if (result.find("V1") < 100){ //如果该点在工作范围内 Gcode = (std::string)"G0 X" + x + " Y" + y + " Z" + z + " F10000" + "\n"; ROS_INFO("%s", Gcode.c_str()); _serial.write(Gcode.c_str()); usleep(100*1000); result = _serial.read(_serial.available()); if (result.find("E26") < 100) //如果目标点可以达到而直线插补不到 { //先将其运动到中间点200,0,100 char x1[10]; char y1[10]; char z1[10]; sprintf(x1, "%.2f", 200.0); sprintf(y1, "%.2f", 0.0); sprintf(z1, "%.2f", 100.0); Gcode = (std::string)"G0 X" + x1 + " Y" + y1 + " Z" + z1 + " F10000" + "\r\n"; ROS_INFO("%s", Gcode.c_str()); _serial.write(Gcode.c_str()); result = _serial.read(_serial.available()); int i=0; while (!(abs(pos.position.x-200.0)<1&&abs(pos.position.y-0.0)<1&&abs(pos.position.z-100.0)<1)) { get_pos(); loop_rate.sleep(); i++; if(i>200)return false; } //然后再运动到目标点 Gcode = (std::string)"G0 X" + x + " Y" + y + " Z" + z + " F10000" + "\n"; _serial.write(Gcode.c_str()); result = _serial.read(_serial.available()); } //等待移动完成 int i=0; while (!(abs(pos.position.x-gx)<1&&abs(pos.position.y-gy)<1&&abs(pos.position.z-gz)<1)) { /* code for loop body */ get_pos(); loop_rate.sleep(); i++; // result = _serial.read(_serial.available()); // if (result.find("@6") < 100&& result.find("V1")){ // return true; // } if(i>200) return false; } return true; }else{ return false; } } //goto pos 回调函数 bool ARM_CONTROL::goto_position_deal(arm_controller::move::Request &req, arm_controller::move::Response &res) { //调用goto 函数 _serial.write("M2122\r\n"); //停止反馈 res.success=goto_pos(req.pose.position.x,req.pose.position.y,req.pose.position.z); _serial.write("M2120 V0.1\r\n"); // report position per 0.05s return true; } //气泵回调函数 bool ARM_CONTROL::pump_callback(std_srvs::SetBool::Request &req,std_srvs::SetBool::Response &res) { std::string Gcode = ""; if (req.data == true) { //打开气泵指令M2231 V1 Gcode = (std::string)"M2231 V1" + "\r\n"; }else{ //关闭气泵指令M2231 V1 Gcode = (std::string)"M2231 V0" + "\r\n"; } ROS_INFO("%s", Gcode.c_str()); int i = _serial.write(Gcode.c_str()); res.success=i; read_data = _serial.read(_serial.available()); cout << read_data << endl; return true; } //解锁上锁电机 bool ARM_CONTROL::unlcok_callback(std_srvs::SetBool::Request &req,std_srvs::SetBool::Response &res) { std::string Gcode = ""; if (req.data == true) { //解锁所有电机 V1 Gcode = (std::string)"M2019" + "\r\n"; }else{ //锁住所有电机 V1 Gcode = (std::string)"M17" + "\r\n"; } ROS_INFO("%s", Gcode.c_str()); int i = _serial.write(Gcode.c_str()); res.success=true; return true; } ARM_CONTROL::ARM_CONTROL() { //服务器 pick pick_server = nh.advertiseService("pump",&ARM_CONTROL::pump_callback,this); //解锁电机 unlock_server = nh.advertiseService("unlock",&ARM_CONTROL::unlcok_callback,this); //服务器 goto_position goto_position_server = nh.advertiseService("goto_position",&ARM_CONTROL::goto_position_deal,this); //发布者 arm_controller/position_info 发布机械臂位置信息 pub = nh.advertise<arm_controller::control>("arm_controller/position_info", 1); //接收相对位移 sub = nh.subscribe("/arm_controller/cmd_vel",2,&ARM_CONTROL::cmd_callback,this); //串口连接 try { _serial.setPort("/dev/ttyACM0"); _serial.setBaudrate(115200); serial::Timeout to = serial::Timeout::simpleTimeout(1000); _serial.setTimeout(to); _serial.open(); ROS_INFO_STREAM("Port has been open successfully"); } catch (serial::IOException& e) { ROS_ERROR_STREAM("Unable to open port"); ros::shutdown(); //return -1; } if (_serial.isOpen()) { ros::Duration(3.5).sleep(); // wait 3.5s _serial.write("M2120 V0\r\n"); // stop report position ros::Duration(0.1).sleep(); // wait 0.1s _serial.write("M17\r\n"); // attach ros::Duration(0.1).sleep(); // wait 0.1s ROS_INFO_STREAM("Attach and wait for commands"); } //返回笛卡尔坐标系 _serial.write("M2120 V0.1\r\n"); // report position per 0.05s //10hz ros::Rate loop_rate(10); //读取初始化信息 while (ros::ok()) { string read_line; _serial.readline(read_line,65535,"\n"); cout << read_line; if (read_line.find("@5 V1") < 1000) break; ros::Duration(0.05).sleep(); // wait 0.1s } //开始主循环 while (ros::ok()) { //获取机械臂位姿函数 get_pos(); //如果机械臂位姿未获取到则不发布消息 if(abs(pos.position.x)<0.1&&abs(pos.position.y)<0.1&&abs(pos.position.z)<0.1) { ros::spinOnce(); loop_rate.sleep(); continue; } ros::spinOnce(); loop_rate.sleep(); } } int main(int argc, char** argv) { ros::init(argc, argv, "arm_controller"); ARM_CONTROL carm; ros::shutdown(); }
07-22
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值