uva_375 - Inscribed Circles and Isosceles Triangles

本文介绍了一种计算等腰三角形中一系列内接圆周长之和的方法,通过递归方式累加直到最小圆半径达到指定阈值。利用三角函数计算每个内接圆的半径,并给出完整的C++实现代码。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

Given two real numbers

B
the width of the base of an isosceles triangle in inches
H
the altitude of the same isosceles triangle in inches

Compute to six significant decimal places

C
the sum of the circumferences of a series of inscribed circles stacked one on top of another from the base to the peak; such that the lowest inscribed circle is tangent to the base and the two sides and the next higher inscribed circle is tangent to the lowest inscribed circle and the two sides, etc. In order to keep the time required to compute the result within reasonable bounds, you may limit the radius of the smallest inscribed circle in the stack to a single precision floating point value of 0.000001.

For those whose geometry and trigonometry are a bit rusty, the center of an inscribed circle is at the point of intersection of the three angular bisectors.

Input

The input begins with a single positive integer on a line by itself indicating the number of the cases following, each of them as described below. This line is followed by a blank line, and there is also a blank line between two consecutive inputs.
The input will be a single line of text containing two positive single precision real numbers (B H) separated by spaces.

Output

For each test case, the output must follow the description below. The outputs of two consecutive cases will be separated by a blank line.
The output should be a single real number with twelve significant digits, six of which follow the decimal point. The decimal point must be printed in column 7.

Sample Input

1

0.263451 0.263451

Sample Output

     0.827648

    不断的累加内接圆面积直至半径r<0.000001. 刚开始用等面积法三角形内接圆半径一直wa,可能是用到开根号算出来有些微小误差。。。

    后来改用cmath里的三角函数求就ac了



#include <iostream>
#include <cmath>
#include <iomanip>
using namespace std;



double circum ( double b, double h)
{
    double theta = atan(2*h/b);     //腰与高的夹角
    double r = b/2*tan(theta/2);   
    if ( r < 0.000001) return 0;

    return 2*M_PI*r + circum( (1-2*r/h)*b, h-2*r);

}

int main()
{
    int t;
    double b, h;

    cin >> t;
    while( t--){
        cin >> b >> h;
        cout << fixed << setprecision(6);
        cout << setw(13) << circum( b, h) << endl;

        if( t) cout << endl;
    }
    return 0;
}


MoveBase::MoveBase(tf2_ros::Buffer &tf) : tf_(tf), as_(NULL), planner_costmap_ros_(NULL), controller_costmap_ros_(NULL), bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"), blp_loader_("nav_core", "nav_core::BaseLocalPlanner"), recovery_loader_("nav_core", "nav_core::RecoveryBehavior"), planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL), runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false) { // as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false); ros::NodeHandle private_nh("~"); ros::NodeHandle nh; recovery_trigger_ = PLANNING_R; // get some parameters that will be global to the move base node std::string local_planner; // private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS")); private_nh.param("move_base/saveMapping", saveMapping, false); private_nh.param("savePath", savePath, std::string("/home/duzhong/dzacs/src/dzglobalplanner/mapping.txt")); private_nh.param("base_local_planner", local_planner, std::string("dwa_local_planner/DWAPlannerROS")); // private_nh.param("base_local_planner", local_planner, std::string("teb_local_planner/TebLocalPlannerROS")); private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link")); private_nh.param("global_costmap/global_frame", global_frame_, std::string("map")); private_nh.param("planner_frequency", planner_frequency_, 1.0); private_nh.param("controller_frequency", controller_frequency_, 3.0); private_nh.param("planner_patience", planner_patience_, 5.0); private_nh.param("controller_patience", controller_patience_, 15.0); private_nh.param("max_planning_retries", max_planning_retries_, -1); // disabled by default printf("savePath----move_base-->-=%s\n\n\n\n", savePath.c_str()); private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0); private_nh.param("oscillation_distance", oscillation_distance_, 0.5); // set up plan triple buffer planner_plan_ = new std::vector<geometry_msgs::PoseStamped>(); latest_plan_ = new std::vector<geometry_msgs::PoseStamped>(); controller_plan_ = new std::vector<geometry_msgs::PoseStamped>(); // set up the planner's thread planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this)); // for commanding the base vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1); current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0); plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1); all_plan_pub_ = private_nh.advertise<nav_msgs::Path>("allplan", 1); ros::NodeHandle action_nh("move_base"); action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1); // we'll provide a mechanism for some people to send goals as PoseStamped messages over a topic // they won't get any useful information back about its status, but this is useful for tools // like nav_view and rviz ros::NodeHandle simple_nh("move_base_simple"); goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1)); // we'll assume the radius of the robot to be consistent with what's specified for the costmaps private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325); private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46); private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_); private_nh.param("conservative_reset_dist", conservative_reset_dist_, 0.21); private_nh.param("shutdown_costmaps", shutdown_costmaps_, false); private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true); private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true); private_nh.param("road_paths_savePath", road_paths_savePath, std::string("/home/duzhong/dzacs/src/resource/road_paths")); private_nh.param("stop_points_savePath", stop_points_savePath, std::string("/home/duzhong/dzacs/src/resource/stop_points")); if (saveMapping) { printf("move_base--> saveMapping = true\n"); } else if (!saveMapping) { printf("move_base--> saveMapping = false\n"); } // create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_); planner_costmap_ros_->pause(); // initialize the global planner /* try { planner_ = bgp_loader_.createInstance(global_planner); planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_); } catch (const pluginlib::PluginlibException &ex) { ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what()); exit(1); } */ // create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_); controller_costmap_ros_->pause(); // create a local planner try { tc_ = blp_loader_.createInstance(local_planner); ROS_INFO("Created local_planner %s", local_planner.c_str()); tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_); } catch (const pluginlib::PluginlibException &ex) { ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what()); exit(1); } // Start actively updating costmaps based on sensor data planner_costmap_ros_->start(); controller_costmap_ros_->start(); // advertise a service for getting a plan make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this); // advertise a service for clearing the costmaps clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this); // if we shutdown our costmaps when we're deactivated... we'll do that now if (shutdown_costmaps_) { ROS_DEBUG_NAMED("move_base", "Stopping costmaps initially"); planner_costmap_ros_->stop(); controller_costmap_ros_->stop(); } // load any user specified recovery behaviors, and if that fails load the defaults if (!loadRecoveryBehaviors(private_nh)) { loadDefaultRecoveryBehaviors(); } // initially, we'll need to make a plan state_ = PLANNING; // we'll start executing recovery behaviors at the beginning of our list recovery_index_ = 0; foundObstacleFlag = 0; pursuitSpeed = 0.6; pubtargetSpeed = 0.6; recvScanFlag = 0; saveMappingMsg.data = 0; road_points_index = 0; stop_point_signal_msg.data = 0; sub_lasersScan = nh.subscribe("scan", 1, &MoveBase::callback_lasersScan, this); sub_saveMapping = nh.subscribe("savemapping", 1, &MoveBase::callback_saveMapping, this); hgglobalplannerpub = private_nh.advertise<move_base_msgs::hgpathplanner>("hgglobalplanner", 10); hglocationpub = private_nh.advertise<move_base_msgs::hglocation>("hglocation", 10); angle_pub = nh.advertise<geometry_msgs::Twist>("pursuitAngle", 1); vis_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10); stop_point_signal = private_nh.advertise<std_msgs::UInt8>("stop_signal",1); std::vector<std::string> mapFolders = { road_paths_savePath, stop_points_savePath}; loadAllMaps(mapFolders); roadPoints = road_paths[road_points_index]; stopPoints = stop_Points[0]; doplanner(); }分析一下这一段ROS代码的用处,我想添加一个接受其他几个节点话题发布的消息,对订阅的消息进行综合判断,进而指定导航路线。我该怎么做,事自己写一个类还是在这个MoseBase里面写
最新发布
07-29
<think>嗯,用户想分析ROS中MoveBase类的代码功能,并扩展功能实现动态导航路径规划。这需要结合ROS框架和C++开发经验,用户应该是个有一定ROS基础的开发者。 用户提到的两个引用很关键。引用1说明move_base是路径规划核心功能包,整合了传感器数据、地图和定位信息;引用2补充了它使用ActionLib机制处理导航目标。但用户实际需求更深入——要修改源码实现动态路径订阅。 先从代码结构入手分析。MoveBase类在move_base包内,核心文件是move_base.cpp和move_base.h。它作为协调器管理全局规划器(如global_planner::GlobalPlanner)、局部规划器(如base_local_planner::TrajectoryPlanner)和恢复行为三大模块。全局规划器通过订阅costmap_2d::Costmap2DROS获取地图数据生成初始路径,局部规划器则根据激光雷达等传感器数据实时避障。 要实现动态路径订阅,需要改造全局规划部分。当前规划器只在收到新目标点时计算一次路径(通过makePlan接口)。用户需要的功能是:在导航过程中实时接收外部路径点更新全局路径。这涉及三个关键扩展点: 1. 新增订阅者:在MoveBase类中添加ros::Subscriber订阅自定义路径消息(如geometry_msgs/PoseArray) 2. 中断当前规划:当收到新路径点时调用planner_->cancel()终止正在进行的规划线程 3. 触发重规划:将新路径点封装为move_base_msgs::MoveBaseGoal并通过executeCb重新启动规划流程 特别注意线程安全!因为executeCb运行在独立线程中,修改目标点需加锁保护。建议用boost::mutex锁住planner_mutex_,避免规划器状态冲突。另外,动态路径可能包含多个航点,需要循环调用全局规划器分段计算路径,同时考虑路径平滑性(可调用nav_core::BaseGlobalPlanner的orientationFilter函数调整航向角)。 性能方面,频繁重规划可能增加CPU负载。建议用户添加距离阈值判断:仅当新路径点与当前目标点距离超过0.5米时才触发重规划。对于动态避障场景,其实更推荐用局部规划器处理,全局路径动态更新可能破坏导航稳定性。 最后提醒用户:修改后务必在仿真环境测试!可结合stage或gazebo模拟动态障碍物,用rostopic pub发送测试路径点验证功能。如果遇到规划器频繁报错,尝试增大planner_window_size参数值提升容错性。</think>以下是对ROS中MoveBase类的代码分析及扩展动态导航路径规划的解决方案: --- ### **1. MoveBase类核心功能分析** MoveBase是ROS导航栈的核心控制器,主要功能包括: ```plantuml @startuml package "MoveBase" { [全局路径规划器] --> [代价地图] [局部路径规划器] --> [传感器数据] [恢复行为模块] --> [错误检测] } [MoveBase] --> [ActionServer] [ActionServer] --> [接收导航目标] [MoveBase] --> [发布速度指令] @enduml ``` **关键组件说明**: 1. **全局规划器**(`global_planner::GlobalPlanner`) - 订阅静态地图(`map_server`)和动态代价地图(`costmap_2d`) - 生成从起点到目标点的全局路径(`nav_msgs/Path`) 2. **局部规划器**(`base_local_planner::TrajectoryPlanner`) - 订阅激光雷达等实时传感器数据 - 生成局部避障路径和速度指令(`geometry_msgs/Twist`) 3. **恢复行为模块** - 处理规划失败场景(如旋转、清除代价地图) 4. **ActionLib接口** - 通过`move_base_msgs/MoveBaseAction`接收导航目标 - 反馈状态(运行中/成功/失败) --- ### **2. 扩展动态路径订阅功能** #### **步骤1:创建自定义消息** 在`msg/`目录下创建`DynamicPath.msg`: ```plaintext Header header geometry_msgs/PoseStamped[] waypoints ``` #### **步骤2:修改MoveBase代码** 在`move_base.h`中添加: ```cpp #include <your_pkg/DynamicPath.h> ros::Subscriber dynamic_path_sub; void dynamicPathCallback(const your_pkg::DynamicPath::ConstPtr& msg); ``` 在`move_base.cpp`中实现回调: ```cpp void MoveBase::dynamicPathCallback(const your_pkg::DynamicPath::ConstPtr& msg){ // 1. 清除当前目标 as_->setPreempted(); // 2. 提取路径点 std::vector<geometry_msgs::PoseStamped> waypoints = msg->waypoints; // 3. 按顺序执行路径点 for(auto& pose : waypoints){ move_base_msgs::MoveBaseGoal goal; goal.target_pose = pose; executeCycle(goal); // 调用内部执行函数 while(as_->isActive()) ros::spinOnce(); // 等待当前点完成 } } ``` #### **步骤3:初始化订阅者** 在`MoveBase`构造函数中添加: ```cpp dynamic_path_sub = nh_.subscribe("dynamic_path", 1, &MoveBase::dynamicPathCallback, this); ``` --- ### **3. 动态路径规划实现逻辑** ```plantuml @startuml actor User participant "Custom Node" as CN participant "MoveBase" as MB participant "Global Planner" as GP User -> CN : 发布DynamicPath消息 CN -> MB : /dynamic_path (订阅) MB -> MB : 中断当前导航 loop 遍历所有路径点 MB -> GP : 请求新路径规划 GP --> MB : 返回路径 MB -> MB : 执行局部规划 MB --> User : 反馈状态 end @enduml ``` --- ### **4. 关键注意事项** 1. **线程安全** - 使用`boost::mutex`保护目标点更新操作 - 在回调函数中加锁:`boost::lock_guard<boost::mutex> lock(planner_mutex_);` 2. **异常处理** ```cpp if(waypoints.empty()){ ROS_ERROR("Received empty path!"); return; } ``` 3. **实时性优化** - 设置路径点超时机制(如30秒未到达则跳过) - 在`executeCycle`中添加障碍物检测中断 --- ### **5. 测试方法** 1. 发布测试路径: ```bash rostopic pub /dynamic_path your_pkg/DynamicPath "header: seq: 0 waypoints: - pose: {position: {x: 1.0, y: 0.0}, orientation: {w: 1.0}} - pose: {position: {x: 2.0, y: 1.0}, orientation: {w: 0.7}}" ``` 2. 监控状态: ```bash rostopic echo /move_base/status ``` --- ### **
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值