对‘tf::TransformListener::TransformListener(ros::Duration, bool)’未定义的引用

carto_tf.cpp:(.text+0x163):对‘tf::Transformer::DEFAULT_CACHE_TIME’未定义的引用

carto_tf.cpp:(.text+0x19f):对‘tf::TransformListener::TransformListener(ros::Duration, bool)’未定义的引用

carto_tf.cpp:(.text+0x2e0):对‘tf::Transformer::waitForTransform(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&, ros::Duration const&, ros::Duration const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >*) const’未定义的引用

carto_tf.cpp:(.text+0x3d3):对‘tf::Transformer::lookupTransform(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&, tf::Stamped

bool pidAdjustCallback(int value) { int taskStep = value; int taskState; ros::Rate loop_rate(10); // 10Hz控制频率 // PID控制变量 double prev_error_x = 0.0, prev_error_y = 0.0, prev_error_yaw = 0.0; double integral_x = 0.0, integral_y = 0.0, integral_yaw = 0.0; tf::TransformListener listener; while (ros::ok()) { if (rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::PAUSE) { rei_cruise::RobotCruise::getInstance().SetCallBackFuncStep(taskStep); return true; } else if (rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::STOP) { return true; } taskState = rei_cruise::RobotCruise::getInstance().GetTaskState(); if (taskState == rei_cruise::TaskState::ACTIVE) { switch (taskStep) { case 0: { // 获取当前位置 tf::StampedTransform transform; try { // 获取从odom到base_link的变换 listener.waitForTransform("odom", "base_link", ros::Time(0), ros::Duration(3.0)); listener.lookupTransform("odom", "base_link", ros::Time(0), transform); // 当前位置 double current_x = transform.getOrigin().x(); double current_y = transform.getOrigin().y(); tf::Quaternion q = transform.getRotation(); double current_yaw = tf::getYaw(q); // 计算误差 double error_x = -current_x; // 目标位置是原点(0,0,0) double error_y = -current_y; double error_yaw = -current_yaw; // 目标朝向0弧度 // 角度归一化到[-π, π] while (error_yaw > M_PI) error_yaw -= 2 * M_PI; while (error_yaw < -M_PI) error_yaw += 2 * M_PI; // 检查是否达到精度要求 if (fabs(error_x) < TOLERANCE_POS && fabs(error_y) < TOLERANCE_POS && fabs(error_yaw) < TOLERANCE_ANGLE) { ROS_INFO("PID adjustment completed. Position accurate."); return true; } // PID计算 integral_x += error_x; integral_y += error_y; integral_yaw += error_yaw; double derivative_x = error_x - prev_error_x; double derivative_y = error_y - prev_error_y; double derivative_yaw = error_yaw - prev_error_yaw; double vel_x = KP_LINEAR * error_x + KI_LINEAR * integral_x + KD_LINEAR * derivative_x; double vel_y = KP_LINEAR * error_y + KI_LINEAR * integral_y + KD_LINEAR * derivative_y; double vel_yaw = KP_ANGULAR * error_yaw + KI_ANGULAR * integral_yaw + KD_ANGULAR * derivative_yaw; // 限制速度范围 vel_x = std::max(std::min(vel_x, 0.3), -0.3); vel_y = std::max(std::min(vel_y, 0.3), -0.3); vel_yaw = std::max(std::min(vel_yaw, 0.5), -0.5); // 发布速度命令 geometry_msgs::Twist cmd_vel; cmd_vel.linear.x = vel_x; cmd_vel.linear.y = vel_y; cmd_vel.angular.z = vel_yaw; if (g_vel_pub_ptr) { g_vel_pub_ptr->publish(cmd_vel); } // 更新前次误差 prev_error_x = error_x; prev_error_y = error_y; prev_error_yaw = error_yaw; ROS_INFO("Adjusting position: dx=%.3f, dy=%.3f, dyaw=%.3f", error_x, error_y, error_yaw); } catch (tf::TransformException &ex) { ROS_ERROR("TF exception: %s", ex.what()); return false; } break; } // 可以添加更多步骤(如果需要) } } else if (taskState == rei_cruise::TaskState::STOP) { break; } loop_rate.sleep(); } return true; }
最新发布
06-29
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值