diff --git a/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp b/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp index 5278fcda731543f4cdb98c8a05e872067d214bef..ee891427aebe42fd391a64efee478ab2e9b260f0 100644 --- a/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp +++ b/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp @@ -59,7 +59,7 @@ namespace { std::shared_ptr<doa::Obstacle> - create_doa_obstacle(const armarx::dsobstacleavoidance::Obstacle& obstacle) + create_doa_obstacle(const armarx::obstacledetection::Obstacle& obstacle) { Eigen::Vector3d pos(obstacle.posX, obstacle.posY, obstacle.posZ); Eigen::Quaterniond ori{Eigen::AngleAxisd(obstacle.yaw, Eigen::Vector3d::UnitZ())}; @@ -87,11 +87,11 @@ namespace } -namespace armarx::dsobstacleavoidance +namespace armarx::obstacledetection { void - from_json(const nlohmann::json& j, Obstacle& obstacle) + from_json(const nlohmann::json& j, armarx::obstacledetection::Obstacle& obstacle) { j.at("name").get_to(obstacle.name); j.at("posX").get_to(obstacle.posX); @@ -176,8 +176,8 @@ armarx::DSObstacleAvoidance::onInitComponent() std::ifstream ifs(scene_obstacles_path); nlohmann::json j = nlohmann::json::parse(ifs); - std::vector<dsobstacleavoidance::Obstacle> obstacles = j; - for (dsobstacleavoidance::Obstacle& obstacle : obstacles) + std::vector<obstacledetection::Obstacle> obstacles = j; + for (obstacledetection::Obstacle& obstacle : obstacles) { if (m_doa.cfg.only_2d) { @@ -272,7 +272,7 @@ armarx::DSObstacleAvoidance::getConfig(const Ice::Current&) void armarx::DSObstacleAvoidance::updateObstacle( - const armarx::dsobstacleavoidance::Obstacle& obstacle, + const armarx::obstacledetection::Obstacle& obstacle, const Ice::Current&) { ARMARX_DEBUG << "Adding/updating obstacle: " << obstacle.name << "."; @@ -313,18 +313,18 @@ armarx::DSObstacleAvoidance::removeObstacle(const std::string& obstacle_name, co } -std::vector<armarx::dsobstacleavoidance::Obstacle> +std::vector<armarx::obstacledetection::Obstacle> armarx::DSObstacleAvoidance::getObstacles(const Ice::Current&) { std::shared_lock l{m_doa.mutex}; - std::vector<dsobstacleavoidance::Obstacle> obstacle_list; + std::vector<obstacledetection::Obstacle> obstacle_list; for (auto& [doa_name, doa_obstacle] : m_doa.env) { std::shared_ptr<doa::Ellipsoid> doa_ellipsoid = std::dynamic_pointer_cast<doa::Ellipsoid>(doa_obstacle); - dsobstacleavoidance::Obstacle obstacle; + obstacledetection::Obstacle obstacle; obstacle.name = doa_name; obstacle.posX = doa_ellipsoid->get_position()(0) * 1000; @@ -409,7 +409,7 @@ armarx::DSObstacleAvoidance::updateEnvironment(const Ice::Current&) // Make working copies of the updates and free them again. std::vector<buffer::update> update_log; - std::map<std::string, dsobstacleavoidance::Obstacle> obstacles; + std::map<std::string, obstacledetection::Obstacle> obstacles; { std::lock_guard l{m_buf.mutex}; @@ -461,7 +461,7 @@ armarx::DSObstacleAvoidance::updateEnvironment(const Ice::Current&) ARMARX_DEBUG << "Adding obstacle " << update.name << "."; } - const dsobstacleavoidance::Obstacle& obstacle = obstacles.at(update.name); + const obstacledetection::Obstacle& obstacle = obstacles.at(update.name); m_doa.env.add_obstacle(::create_doa_obstacle(obstacle)); } break; @@ -504,7 +504,7 @@ armarx::DSObstacleAvoidance::writeDebugPlots(const std::string& filename, const void armarx::DSObstacleAvoidance::sanity_check_obstacle( - const armarx::dsobstacleavoidance::Obstacle& obstacle) + const armarx::obstacledetection::Obstacle& obstacle) const { ARMARX_DEBUG << "Sanity checking obstacle `" << obstacle.name << "`."; @@ -553,7 +553,7 @@ armarx::DSObstacleAvoidance::run_visualization() color_m = Color::blue(255, 64); } - for (const dsobstacleavoidance::Obstacle& obstacle : getObstacles()) + for (const obstacledetection::Obstacle& obstacle : getObstacles()) { const double safetyMarginZ = m_doa.cfg.only_2d ? 1 : obstacle.safetyMarginZ; const double posZ = m_doa.cfg.only_2d ? 1 : obstacle.posZ; diff --git a/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.h b/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.h index d1faa6164f607953c8da5736d8998cc6dfae2b5f..2f9035f45eb97f749f17e083ae8a150abf061ada 100644 --- a/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.h +++ b/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.h @@ -79,11 +79,11 @@ namespace armarx void updateObstacle( - const dsobstacleavoidance::Obstacle& obstacle, + const obstacledetection::Obstacle& obstacle, const Ice::Current& = Ice::emptyCurrent) override; - std::vector<dsobstacleavoidance::Obstacle> + std::vector<obstacledetection::Obstacle> getObstacles(const Ice::Current& = Ice::emptyCurrent) override; @@ -147,7 +147,7 @@ namespace armarx private: void - sanity_check_obstacle(const dsobstacleavoidance::Obstacle& obstacle) + sanity_check_obstacle(const obstacledetection::Obstacle& obstacle) const; void @@ -205,7 +205,7 @@ namespace armarx mutable std::mutex mutex; std::vector<DSObstacleAvoidance::buffer::update> update_log; - std::map<std::string, dsobstacleavoidance::Obstacle> obstacles; + std::map<std::string, obstacledetection::Obstacle> obstacles; bool needs_env_update; IceUtil::Time last_env_update; };