diff --git a/ocs2_core/src/integration/IntegratorBase.cpp b/ocs2_core/src/integration/IntegratorBase.cpp index 617685734..2979b766d 100644 --- a/ocs2_core/src/integration/IntegratorBase.cpp +++ b/ocs2_core/src/integration/IntegratorBase.cpp @@ -178,7 +178,7 @@ void IntegratorBase::integrateTimes(OdeBase& system, } catch (const std::runtime_error& error) { - std::cout << "[IntegratorBase::integrateTimes] INTEGRATION TERMINATED!!!" << std::endl << std::endl; + //std::cout << "[IntegratorBase::integrateTimes] INTEGRATION TERMINATED!!!" << std::endl << std::endl; //std::cout << "[IntegratorBase::integrateTimes] DEBUG INF" << std::endl << std::endl; //while(1); } diff --git a/ocs2_core/src/thread_support/ThreadPool.cpp b/ocs2_core/src/thread_support/ThreadPool.cpp index b64f1de8f..8b8b9a4a5 100644 --- a/ocs2_core/src/thread_support/ThreadPool.cpp +++ b/ocs2_core/src/thread_support/ThreadPool.cpp @@ -103,13 +103,13 @@ void ThreadPool::runTask(std::unique_ptr taskPtr) { /**************************************************************************************************/ void ThreadPool::runParallel(std::function taskFunction, int N) { - std::cout << "[ThreadPool::runParallel] START" << std::endl; + //std::cout << "[ThreadPool::runParallel] START" << std::endl; // Launch tasks in helper threads std::vector> futures; if (N > 1) { - std::cout << "[ThreadPool::runParallel] GALAPAKOS 0" << std::endl; + //std::cout << "[ThreadPool::runParallel] GALAPAKOS 0" << std::endl; const int numHelpers = N - 1; futures.reserve(numHelpers); for (int i = 0; i < numHelpers; ++i) @@ -125,11 +125,11 @@ void ThreadPool::runParallel(std::function taskFunction, int N) // Wait for helpers to finish. for (auto&& fut : futures) { - std::cout << "[ThreadPool::runParallel] GALAPAKOS 1" << std::endl; + //std::cout << "[ThreadPool::runParallel] GALAPAKOS 1" << std::endl; fut.get(); } - std::cout << "[ThreadPool::runParallel] END" << std::endl; + //std::cout << "[ThreadPool::runParallel] END" << std::endl; } } // namespace ocs2 diff --git a/ocs2_ddp/src/GaussNewtonDDP.cpp b/ocs2_ddp/src/GaussNewtonDDP.cpp index 4567449d3..d64ad77da 100644 --- a/ocs2_ddp/src/GaussNewtonDDP.cpp +++ b/ocs2_ddp/src/GaussNewtonDDP.cpp @@ -150,14 +150,10 @@ GaussNewtonDDP::GaussNewtonDDP(ddp::Settings ddpSettings, /******************************************************************************************************/ GaussNewtonDDP::~GaussNewtonDDP() { - std::cerr << getBenchmarkingInfo() << std::endl; - - /* if (ddpSettings_.displayInfo_ || ddpSettings_.displayShortSummary_) { std::cerr << getBenchmarkingInfo() << std::endl; } - */ } /******************************************************************************************************/ @@ -1270,7 +1266,7 @@ void GaussNewtonDDP::initializeDualSolutionAndMetrics() /******************************************************************************************************/ void GaussNewtonDDP::takePrimalDualStep(scalar_t lqModelExpectedCost) { - std::cout << "[GaussNewtonDDP::takePrimalDualStep] START" << std::endl; + //std::cout << "[GaussNewtonDDP::takePrimalDualStep] START" << std::endl; // update primal: run search strategy and find the optimal stepLength searchStrategyTimer_.startTimer(); @@ -1307,7 +1303,7 @@ void GaussNewtonDDP::takePrimalDualStep(scalar_t lqModelExpectedCost) solution); //*/ - std::cout << "[GaussNewtonDDP::takePrimalDualStep] END searchStrategyPtr_" << std::endl; + //std::cout << "[GaussNewtonDDP::takePrimalDualStep] END searchStrategyPtr_" << std::endl; if (success) { @@ -1626,7 +1622,7 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, const // DDP main loop while (true) { - std::cout << "[GaussNewtonDDP::runImpl(4)] START DDP LOOP" << std::endl; + //std::cout << "[GaussNewtonDDP::runImpl(4)] START DDP LOOP" << std::endl; if (ddpSettings_.displayInfo_) { @@ -1649,21 +1645,21 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, const backwardPassTimer_.endTimer(); //std::cout << "[GaussNewtonDDP::runImpl(4)] END solveSequentialRiccatiEquations" << std::endl; - std::cout << "[GaussNewtonDDP::runImpl(4)] START calculateController" << std::endl; + //std::cout << "[GaussNewtonDDP::runImpl(4)] START calculateController" << std::endl; // calculate controller and store the result in unoptimizedController_ computeControllerTimer_.startTimer(); calculateController(); computeControllerTimer_.endTimer(); - std::cout << "[GaussNewtonDDP::runImpl(4)] END calculateController" << std::endl; + //std::cout << "[GaussNewtonDDP::runImpl(4)] END calculateController" << std::endl; // the expected cost/merit calculated by the Riccati solution is not reliable const auto lqModelExpectedCost = initialSolutionExists ? nominalDualData_.valueFunctionTrajectory.front().f : performanceIndex_.merit; - std::cout << "[GaussNewtonDDP::runImpl(4)] START takePrimalDualStep" << std::endl; + //std::cout << "[GaussNewtonDDP::runImpl(4)] START takePrimalDualStep" << std::endl; // nominal --> optimized: based on the current LQ solution updates the optimized primal and dual solutions takePrimalDualStep(lqModelExpectedCost); - std::cout << "[GaussNewtonDDP::runImpl(4)] END takePrimalDualStep" << std::endl; + //std::cout << "[GaussNewtonDDP::runImpl(4)] END takePrimalDualStep" << std::endl; // iteration info ++totalNumIterations_; @@ -1679,11 +1675,11 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, const std::tie(isConverged, convergenceInfo) = searchStrategyPtr_->checkConvergence(!initialSolutionExists, *std::prev(performanceIndexHistory_.end(), 2), performanceIndexHistory_.back()); initialSolutionExists = true; - std::cout << "[GaussNewtonDDP::runImpl(4)] convergenceInfo: " << std::endl; + //std::cout << "[GaussNewtonDDP::runImpl(4)] convergenceInfo: " << std::endl; //std::cout << convergenceInfo << std::endl; if (isConverged || (totalNumIterations_ - initIteration) == ddpSettings_.maxNumIterations_) { - std::cout << "[GaussNewtonDDP::runImpl(4)] END DDP LOOP" << std::endl; + //std::cout << "[GaussNewtonDDP::runImpl(4)] END DDP LOOP" << std::endl; break; } else @@ -1703,7 +1699,7 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, const optimizedPrimalSolution_.swap(nominalPrimalData_.primalSolution); optimizedProblemMetrics_.swap(nominalPrimalData_.problemMetrics); - std::cout << "[GaussNewtonDDP::runImpl(4)] CONTINUE DDP LOOP" << std::endl << std::endl;; + //std::cout << "[GaussNewtonDDP::runImpl(4)] CONTINUE DDP LOOP" << std::endl << std::endl;; } //ctr++; } // end of while loop diff --git a/ocs2_ddp/src/search_strategy/LineSearchStrategy.cpp b/ocs2_ddp/src/search_strategy/LineSearchStrategy.cpp index d580e3ae4..f9ab135fc 100644 --- a/ocs2_ddp/src/search_strategy/LineSearchStrategy.cpp +++ b/ocs2_ddp/src/search_strategy/LineSearchStrategy.cpp @@ -162,7 +162,7 @@ void LineSearchStrategy::computeSolution(size_t taskId, const vector_t& initFullState, search_strategy::Solution& solution) { - std::cout << "[LineSearchStrategy::computeSolution(4)] START" << std::endl; + //std::cout << "[LineSearchStrategy::computeSolution(4)] START" << std::endl; auto& problem = optimalControlProblemRefStock_[taskId]; auto& rollout = rolloutRefStock_[taskId]; @@ -223,7 +223,7 @@ void LineSearchStrategy::computeSolution(size_t taskId, printString(infoDisplay.str()); } - std::cout << "[LineSearchStrategy::computeSolution(4)] END" << std::endl; + //std::cout << "[LineSearchStrategy::computeSolution(4)] END" << std::endl; } /******************************************************************************************************/ @@ -309,7 +309,7 @@ bool LineSearchStrategy::run(const std::pair& timePeriod, const ModeSchedule& modeSchedule, search_strategy::SolutionRef solutionRef) { - std::cout << "[LineSearchStrategy::run(8)] START" << std::endl; + //std::cout << "[LineSearchStrategy::run(8)] START" << std::endl; // initialize lineSearchModule inputs lineSearchInputRef_.timePeriodPtr = &timePeriod; @@ -325,10 +325,10 @@ bool LineSearchStrategy::run(const std::pair& timePeriod, try { - std::cout << "[LineSearchStrategy::run(8)] START computeSolution" << std::endl; + //std::cout << "[LineSearchStrategy::run(8)] START computeSolution" << std::endl; //computeSolution(taskId, stepLength, workersSolution_[taskId]); computeSolution(taskId, stepLength, initFullState, workersSolution_[taskId]); - std::cout << "[LineSearchStrategy::run(8)] END computeSolution" << std::endl; + //std::cout << "[LineSearchStrategy::run(8)] END computeSolution" << std::endl; baselineMerit_ = workersSolution_[taskId].performanceIndex.merit; unoptimizedControllerUpdateIS_ = computeControllerUpdateIS(unoptimizedController); @@ -345,22 +345,22 @@ bool LineSearchStrategy::run(const std::pair& timePeriod, " is terminated: " + error.what() + '\n'); } - std::cout << "[LineSearchStrategy::run(8)] BEFORE internalShutDownFlag_: " << internalShutDownFlag_ << std::endl; + //std::cout << "[LineSearchStrategy::run(8)] BEFORE internalShutDownFlag_: " << internalShutDownFlag_ << std::endl; internalShutDownFlag_ = true; - std::cout << "[LineSearchStrategy::run(8)] AFTER internalShutDownFlag_: " << internalShutDownFlag_ << std::endl; + //std::cout << "[LineSearchStrategy::run(8)] AFTER internalShutDownFlag_: " << internalShutDownFlag_ << std::endl; //throw std::runtime_error("[LineSearchStrategy::run(8)] DDP controller does not generate a stable rollout!"); } - std::cout << "[LineSearchStrategy::run(8)] START lineSearchTask" << std::endl; + //std::cout << "[LineSearchStrategy::run(8)] START lineSearchTask" << std::endl; // run workers nextTaskId_ = 0; alphaExpNext_ = 0; alphaProcessed_ = std::vector(maxNumOfSearches(), false); //auto task = [&](int) { lineSearchTask(nextTaskId_++); }; auto task = [&](int) { lineSearchTask(nextTaskId_++, initFullState); }; - std::cout << "[LineSearchStrategy::run(8)] numThreads: " << threadPoolRef_.numThreads() << std::endl; + //std::cout << "[LineSearchStrategy::run(8)] numThreads: " << threadPoolRef_.numThreads() << std::endl; threadPoolRef_.runParallel(task, threadPoolRef_.numThreads()); - std::cout << "[LineSearchStrategy::run(8)] END lineSearchTask" << std::endl; + //std::cout << "[LineSearchStrategy::run(8)] END lineSearchTask" << std::endl; //std::cout << "[LineSearchStrategy::run(8)] START reactivateRollout" << std::endl; // revitalize all integrators @@ -376,7 +376,7 @@ bool LineSearchStrategy::run(const std::pair& timePeriod, std::cerr << "[LineSearchStrategy::run(8)] The chosen step length is: " + std::to_string(bestStepSize_) << "\n"; } - std::cout << "[LineSearchStrategy::run(8)] END" << std::endl; + //std::cout << "[LineSearchStrategy::run(8)] END" << std::endl; return true; } @@ -477,12 +477,12 @@ void LineSearchStrategy::lineSearchTask(const size_t taskId) /******************************************************************************************************/ void LineSearchStrategy::lineSearchTask(const size_t taskId, const vector_t& initFullState) { - std::cout << "[LineSearchStrategy::lineSearchTask(2)] START" << std::endl; + //std::cout << "[LineSearchStrategy::lineSearchTask(2)] START" << std::endl; int ctr = 0; while (true) { - std::cout << "[LineSearchStrategy::lineSearchTask(2)] ctr: " << ctr << std::endl; + //std::cout << "[LineSearchStrategy::lineSearchTask(2)] ctr: " << ctr << std::endl; const size_t alphaExp = alphaExpNext_++; const scalar_t stepLength = settings_.maxStepLength * std::pow(settings_.contractionRate, alphaExp); @@ -494,14 +494,14 @@ void LineSearchStrategy::lineSearchTask(const size_t taskId, const vector_t& ini */ if (!numerics::almost_ge(stepLength, settings_.minStepLength)) { - std::cout << "[LineSearchStrategy::lineSearchTask(2)] BEFORE break 0" << std::endl; + //std::cout << "[LineSearchStrategy::lineSearchTask(2)] BEFORE break 0" << std::endl; break; } // skip if the current learning rate is less than the best candidate if (stepLength < bestStepSize_) { - std::cout << "[LineSearchStrategy::lineSearchTask(2)] BEFORE break 1" << std::endl; + //std::cout << "[LineSearchStrategy::lineSearchTask(2)] BEFORE break 1" << std::endl; // display if (baseSettings_.displayInfo) { @@ -515,7 +515,7 @@ void LineSearchStrategy::lineSearchTask(const size_t taskId, const vector_t& ini try { - std::cout << "[LineSearchStrategy::lineSearchTask(2)] BEFORE computeSolution" << std::endl; + //std::cout << "[LineSearchStrategy::lineSearchTask(2)] BEFORE computeSolution" << std::endl; //computeSolution(taskId, stepLength, workersSolution_[taskId]); computeSolution(taskId, stepLength, initFullState, workersSolution_[taskId]); } @@ -543,7 +543,6 @@ void LineSearchStrategy::lineSearchTask(const size_t taskId, const vector_t& ini const bool armijoCondition = workersSolution_[taskId].performanceIndex.merit < (baselineMerit_ - settings_.armijoCoefficient * stepLength * unoptimizedControllerUpdateIS_); if (armijoCondition && stepLength > bestStepSize_) { // save solution - std::cout << "[LineSearchStrategy::lineSearchTask(2)] ARMIJOOOOO" << std::endl; bestStepSize_ = stepLength; swap(*bestSolutionRef_, workersSolution_[taskId]); terminateLinesearchTasks = std::all_of(alphaProcessed_.cbegin(), alphaProcessed_.cbegin() + alphaExp, [](bool f) { return f; }); @@ -557,7 +556,7 @@ void LineSearchStrategy::lineSearchTask(const size_t taskId, const vector_t& ini { for (RolloutBase& rollout : rolloutRefStock_) { - std::cout << "[LineSearchStrategy::lineSearchTask(2)] BEFORE abortRollout" << std::endl; + //std::cout << "[LineSearchStrategy::lineSearchTask(2)] BEFORE abortRollout" << std::endl; rollout.abortRollout(); } @@ -570,7 +569,7 @@ void LineSearchStrategy::lineSearchTask(const size_t taskId, const vector_t& ini ctr++; } // end of while loop - std::cout << "[LineSearchStrategy::lineSearchTask(2)] END" << std::endl; + //std::cout << "[LineSearchStrategy::lineSearchTask(2)] END" << std::endl; } /******************************************************************************************************/ diff --git a/ocs2_pinocchio/ocs2_ext_collision/src/ExtCollisionCppAd.cpp b/ocs2_pinocchio/ocs2_ext_collision/src/ExtCollisionCppAd.cpp index a95a0db9e..fd998fadf 100644 --- a/ocs2_pinocchio/ocs2_ext_collision/src/ExtCollisionCppAd.cpp +++ b/ocs2_pinocchio/ocs2_ext_collision/src/ExtCollisionCppAd.cpp @@ -38,7 +38,7 @@ ExtCollisionCppAd::ExtCollisionCppAd(const PinocchioInterface& pinocchioInterfac distances_(pointsOnRobotPtr->getNumOfPoints()), emuPtr_(emuPtr) { - std::cout << "[ExtCollisionCppAd::ExtCollisionCppAd] START" << std::endl; + //std::cout << "[ExtCollisionCppAd::ExtCollisionCppAd] START" << std::endl; normalize_flag_ = true; @@ -57,7 +57,7 @@ ExtCollisionCppAd::ExtCollisionCppAd(const PinocchioInterface& pinocchioInterfac cppAdInterfaceDistanceCalculation_->loadModelsIfAvailable(CppAdInterface::ApproximationOrder::First, verbose); } - std::cout << "[ExtCollisionCppAd::ExtCollisionCppAd] END" << std::endl; + //std::cout << "[ExtCollisionCppAd::ExtCollisionCppAd] END" << std::endl; } /******************************************************************************************************/ diff --git a/ocs2_pinocchio/ocs2_ext_collision/src/PointsOnRobot.cpp b/ocs2_pinocchio/ocs2_ext_collision/src/PointsOnRobot.cpp index 09cc48fc6..8efc5484a 100644 --- a/ocs2_pinocchio/ocs2_ext_collision/src/PointsOnRobot.cpp +++ b/ocs2_pinocchio/ocs2_ext_collision/src/PointsOnRobot.cpp @@ -21,7 +21,7 @@ PointsOnRobot::PointsOnRobot(const PointsOnRobot::points_radii_t& points_radii) : points_(), radii_() { - std::cout << "[PointsOnRobot::PointsOnRobot] START" << std::endl; + //std::cout << "[PointsOnRobot::PointsOnRobot] START" << std::endl; const auto& pointsAndRadii = points_radii; @@ -51,7 +51,7 @@ PointsOnRobot::PointsOnRobot(const PointsOnRobot::points_radii_t& points_radii) //nh_ = nh; //pointsOnRobot_visu_pub_ = nh_.advertise("points_on_robot", 1); - std::cout << "[PointsOnRobot::PointsOnRobot] END" << std::endl; + //std::cout << "[PointsOnRobot::PointsOnRobot] END" << std::endl; } //------------------------------------------------------------------------------------------------------- @@ -128,11 +128,13 @@ void PointsOnRobot::initialize(ocs2::PinocchioInterface& pinocchioInterface, break; } - //std::cout << "[PointsOnRobot::initialize] frameNames_: " << std::endl; + /* + std::cout << "[PointsOnRobot::initialize] frameNames_: " << std::endl; for (size_t i = 0; i < frameNames_.size(); i++) { std::cout << i << " -> " << frameNames_[i] << std::endl; } + */ //std::cout << "[PointsOnRobot::initialize] DEBUG INF" << std::endl; //while(1); @@ -418,7 +420,7 @@ Eigen::Matrix PointsOnRobot::QuaternionToEuler(Eigen::Quat Eigen::Matrix euler; euler = quat.toRotationMatrix().eulerAngles(3,2,1); - std::cout << "[PointsOnRobot::QuaternionToEuler] Euler from quaternion in yaw, pitch, roll"<< std::endl << euler << std::endl; + //std::cout << "[PointsOnRobot::QuaternionToEuler] Euler from quaternion in yaw, pitch, roll"<< std::endl << euler << std::endl; return euler; } diff --git a/ocs2_pinocchio/ocs2_ext_collision/src/ext_map_utility.cpp b/ocs2_pinocchio/ocs2_ext_collision/src/ext_map_utility.cpp index 3971a3645..8d55cd45f 100755 --- a/ocs2_pinocchio/ocs2_ext_collision/src/ext_map_utility.cpp +++ b/ocs2_pinocchio/ocs2_ext_collision/src/ext_map_utility.cpp @@ -2504,7 +2504,7 @@ void ExtMapUtility::subscribeOctMsg(string oct_msg_name) //std::cout << "[ExtMapUtility::subscribeOctMsg] oct_msg_name: " << oct_msg_name << std::endl; sub_oct_msg_ = nh_.subscribe(oct_msg_name, 10, &ExtMapUtility::octMsgCallback, this); - std::cout << "[ExtMapUtility::subscribeOctMsg] Waiting for " << oct_msg_name << "..." << std::endl; + //std::cout << "[ExtMapUtility::subscribeOctMsg] Waiting for " << oct_msg_name << "..." << std::endl; boost::shared_ptr octMsgWait = ros::topic::waitForMessage(oct_msg_name); } @@ -2515,56 +2515,56 @@ void ExtMapUtility::subscribeObjectsOctMsg(string obj_oct_msg_name) { if (obj_oct_msg_name == "oct_conveyor") { - std::cout << "[ExtMapUtility::subscribeObjectsOctMsg] oct_msg_name: " << obj_oct_msg_name << std::endl; + //std::cout << "[ExtMapUtility::subscribeObjectsOctMsg] oct_msg_name: " << obj_oct_msg_name << std::endl; sub_oct_msg_conveyor_ = nh_.subscribe(obj_oct_msg_name, 10, &ExtMapUtility::conveyorOctMsgCallback, this); } else if (obj_oct_msg_name == "oct_normal_pkg") { - std::cout << "[ExtMapUtility::subscribeObjectsOctMsg] oct_msg_name: " << obj_oct_msg_name << std::endl; + //std::cout << "[ExtMapUtility::subscribeObjectsOctMsg] oct_msg_name: " << obj_oct_msg_name << std::endl; sub_oct_msg_normal_pkg_ = nh_.subscribe(obj_oct_msg_name, 10, &ExtMapUtility::normalPkgOctMsgCallback, this); } else if (obj_oct_msg_name == "oct_long_pkg") { - std::cout << "[ExtMapUtility::subscribeObjectsOctMsg] oct_msg_name: " << obj_oct_msg_name << std::endl; + //std::cout << "[ExtMapUtility::subscribeObjectsOctMsg] oct_msg_name: " << obj_oct_msg_name << std::endl; sub_oct_msg_long_pkg_ = nh_.subscribe(obj_oct_msg_name, 10, &ExtMapUtility::longPkgOctMsgCallback, this); } else if (obj_oct_msg_name == "oct_longwide_pkg") { - std::cout << "[ExtMapUtility::subscribeObjectsOctMsg] oct_msg_name: " << obj_oct_msg_name << std::endl; + //std::cout << "[ExtMapUtility::subscribeObjectsOctMsg] oct_msg_name: " << obj_oct_msg_name << std::endl; sub_oct_msg_longwide_pkg_ = nh_.subscribe(obj_oct_msg_name, 10, &ExtMapUtility::longwidePkgOctMsgCallback, this); } else if (obj_oct_msg_name == "oct_red_cube") { - std::cout << "[ExtMapUtility::subscribeObjectsOctMsg] oct_msg_name: " << obj_oct_msg_name << std::endl; + //std::cout << "[ExtMapUtility::subscribeObjectsOctMsg] oct_msg_name: " << obj_oct_msg_name << std::endl; sub_oct_msg_red_cube_ = nh_.subscribe(obj_oct_msg_name, 10, &ExtMapUtility::redCubeOctMsgCallback, this); } else if (obj_oct_msg_name == "oct_green_cube") { - std::cout << "[ExtMapUtility::subscribeObjectsOctMsg] oct_msg_name: " << obj_oct_msg_name << std::endl; + //std::cout << "[ExtMapUtility::subscribeObjectsOctMsg] oct_msg_name: " << obj_oct_msg_name << std::endl; sub_oct_msg_green_cube_ = nh_.subscribe(obj_oct_msg_name, 10, &ExtMapUtility::greenCubeOctMsgCallback, this); } else if (obj_oct_msg_name == "oct_blue_cube") { - std::cout << "[ExtMapUtility::subscribeObjectsOctMsg] oct_msg_name: " << obj_oct_msg_name << std::endl; + //std::cout << "[ExtMapUtility::subscribeObjectsOctMsg] oct_msg_name: " << obj_oct_msg_name << std::endl; sub_oct_msg_blue_cube_ = nh_.subscribe(obj_oct_msg_name, 10, &ExtMapUtility::blueCubeOctMsgCallback, this); } else if (obj_oct_msg_name == "oct_actor0") { - std::cout << "[ExtMapUtility::subscribeObjectsOctMsg] oct_msg_name: " << obj_oct_msg_name << std::endl; + //std::cout << "[ExtMapUtility::subscribeObjectsOctMsg] oct_msg_name: " << obj_oct_msg_name << std::endl; sub_oct_msg_actor0_ = nh_.subscribe(obj_oct_msg_name, 10, &ExtMapUtility::actor0OctMsgCallback, this); } else if (obj_oct_msg_name == "oct_actor1") { - std::cout << "[ExtMapUtility::subscribeObjectsOctMsg] oct_msg_name: " << obj_oct_msg_name << std::endl; + //std::cout << "[ExtMapUtility::subscribeObjectsOctMsg] oct_msg_name: " << obj_oct_msg_name << std::endl; sub_oct_msg_actor1_ = nh_.subscribe(obj_oct_msg_name, 10, &ExtMapUtility::actor1OctMsgCallback, this); } else if (obj_oct_msg_name == "oct_bin") { - std::cout << "[ExtMapUtility::subscribeObjectsOctMsg] oct_msg_name: " << obj_oct_msg_name << std::endl; + //std::cout << "[ExtMapUtility::subscribeObjectsOctMsg] oct_msg_name: " << obj_oct_msg_name << std::endl; sub_oct_msg_bin_ = nh_.subscribe(obj_oct_msg_name, 10, &ExtMapUtility::binOctMsgCallback, this); } - std::cout << "[ExtMapUtility::subscribeObjectsOctMsg] Waiting for " << obj_oct_msg_name << "..." << std::endl; + //std::cout << "[ExtMapUtility::subscribeObjectsOctMsg] Waiting for " << obj_oct_msg_name << "..." << std::endl; boost::shared_ptr octMsgWait = ros::topic::waitForMessage(obj_oct_msg_name); } @@ -2594,14 +2594,14 @@ void ExtMapUtility::pointcloud2ToOctPc2(const sensor_msgs::PointCloud2& cloud_pc //------------------------------------------------------------------------------------------------------- void ExtMapUtility::publishOctMsg() { - std::cout << "[ExtMapUtility::publishOctMsg] START" << std::endl; + //std::cout << "[ExtMapUtility::publishOctMsg] START" << std::endl; oct_msg.header.frame_id = world_frame_name; //oct_msg.header.seq++; oct_msg.header.stamp = ros::Time::now(); pub_oct_msg_.publish(oct_msg); - std::cout << "[ExtMapUtility::publishOctMsg] END" << std::endl; + //std::cout << "[ExtMapUtility::publishOctMsg] END" << std::endl; } //------------------------------------------------------------------------------------------------------- diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/include/ocs2_mobile_manipulator/MobileManipulatorInterface.h b/ocs2_robotic_examples/ocs2_mobile_manipulator/include/ocs2_mobile_manipulator/MobileManipulatorInterface.h index cb646d33c..c34a07532 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/include/ocs2_mobile_manipulator/MobileManipulatorInterface.h +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/include/ocs2_mobile_manipulator/MobileManipulatorInterface.h @@ -317,7 +317,9 @@ class MobileManipulatorInterface final : public RobotInterface const std::string taskFile_; const std::string libraryFolder_; const std::string urdfFile_; - std::string robotModelName_ = "mobile_manipulator"; + + /// NUA TODO: ADD NAMESPACE IN robotModelName_ AND ADD THAT INTO CONFIG!!!! + //std::string robotModelName_ = "mobile_manipulator"; std::string worldFrameName_ = "world"; std::string goalFrameName_; diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/src/FactoryFunctions.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator/src/FactoryFunctions.cpp index 97f0385bb..c0037a1a0 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/src/FactoryFunctions.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/src/FactoryFunctions.cpp @@ -128,13 +128,13 @@ PinocchioInterface createPinocchioInterface(const std::string& robotUrdfPath, tf::StampedTransform transform_base_wrt_world; pinocchio::SE3 se3_base_wrt_world = pinocchio::SE3::Identity(); - std::cout << "[FactoryFunctions::createPinocchioInterface(5)] base_frame_name: " << base_frame_name << std::endl; - std::cout << "[FactoryFunctions::createPinocchioInterface(5)] world_frame_name: " << world_frame_name << std::endl; - std::cout << "[FactoryFunctions::createPinocchioInterface(5)] Waiting for the transformation between base and world..." << std::endl; + //std::cout << "[FactoryFunctions::createPinocchioInterface(5)] base_frame_name: " << base_frame_name << std::endl; + //std::cout << "[FactoryFunctions::createPinocchioInterface(5)] world_frame_name: " << world_frame_name << std::endl; + //std::cout << "[FactoryFunctions::createPinocchioInterface(5)] Waiting for the transformation between base and world..." << std::endl; try { - while(!tflistener.waitForTransform(base_frame_name, world_frame_name, ros::Time(0), ros::Duration(1.0))){ros::spinOnce();} - tflistener.lookupTransform(base_frame_name, world_frame_name, ros::Time(0), transform_base_wrt_world); + while(!tflistener.waitForTransform(world_frame_name, base_frame_name, ros::Time(0), ros::Duration(1.0))){ros::spinOnce();} + tflistener.lookupTransform(world_frame_name, base_frame_name, ros::Time(0), transform_base_wrt_world); } catch (tf::TransformException ex) { @@ -151,9 +151,9 @@ PinocchioInterface createPinocchioInterface(const std::string& robotUrdfPath, //se3_base_wrt_world.translation().z() = 0; - std::cout << "[FactoryFunctions::createPinocchioInterface(5)] x: " << se3_base_wrt_world.translation().x() << std::endl; - std::cout << "[FactoryFunctions::createPinocchioInterface(5)] y: " << se3_base_wrt_world.translation().y() << std::endl; - std::cout << "[FactoryFunctions::createPinocchioInterface(5)] z: " << se3_base_wrt_world.translation().z() << std::endl; + //std::cout << "[FactoryFunctions::createPinocchioInterface(5)] x: " << se3_base_wrt_world.translation().x() << std::endl; + //std::cout << "[FactoryFunctions::createPinocchioInterface(5)] y: " << se3_base_wrt_world.translation().y() << std::endl; + //std::cout << "[FactoryFunctions::createPinocchioInterface(5)] z: " << se3_base_wrt_world.translation().z() << std::endl; //std::cout << "[FactoryFunctions::createPinocchioInterface(5)] DEBUG INF" << std::endl; //while(1); diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/src/MobileManipulatorInterface.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator/src/MobileManipulatorInterface.cpp index 546e70877..bfd1e11f5 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/src/MobileManipulatorInterface.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/src/MobileManipulatorInterface.cpp @@ -64,7 +64,7 @@ MobileManipulatorInterface::MobileManipulatorInterface(const std::string& taskFi boost::filesystem::path taskFilePath(taskFile_); if (boost::filesystem::exists(taskFilePath)) { - std::cerr << "[MobileManipulatorInterface::MobileManipulatorInterface] Loading task file: " << taskFilePath << std::endl; + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] Loading task file: " << taskFilePath << std::endl; } else { @@ -75,7 +75,7 @@ MobileManipulatorInterface::MobileManipulatorInterface(const std::string& taskFi boost::filesystem::path urdfFilePath(urdfFile_); if (boost::filesystem::exists(urdfFilePath)) { - std::cerr << "[MobileManipulatorInterface::MobileManipulatorInterface] Loading Pinocchio model from: " << urdfFilePath << std::endl; + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] Loading Pinocchio model from: " << urdfFilePath << std::endl; } else { @@ -85,7 +85,7 @@ MobileManipulatorInterface::MobileManipulatorInterface(const std::string& taskFi /// Create library folder if it does not exist boost::filesystem::path libraryFolderPath(libraryFolder_); boost::filesystem::create_directories(libraryFolderPath); - //std::cerr << "[MobileManipulatorInterface::MobileManipulatorInterface] Generated library path: " << libraryFolderPath << std::endl; + //std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] Generated library path: " << libraryFolderPath << std::endl; /// Read the task file boost::property_tree::ptree pt; @@ -101,7 +101,9 @@ MobileManipulatorInterface::MobileManipulatorInterface(const std::string& taskFi // Read the link names of joints std::vector armJointFrameNames; + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] BEFORE armJointFrameNames" << std::endl; loadData::loadStdVector(taskFile_, "model_information.armJointFrameNames", armJointFrameNames, printOutFlag_); + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] AFTER armJointFrameNames" << std::endl; // Read the names of joints std::vector armJointNames; @@ -110,7 +112,6 @@ MobileManipulatorInterface::MobileManipulatorInterface(const std::string& taskFi // Read the frame names std::string robotName, baseFrame, armBaseFrame, eeFrame; loadData::loadPtreeValue(pt, sim_, "model_information.sim", printOutFlag_); - loadData::loadPtreeValue(pt, ns_, "model_information.ns", printOutFlag_); loadData::loadPtreeValue(pt, robotName, "model_information.robotName", printOutFlag_); loadData::loadPtreeValue(pt, baseFrame, "model_information.baseFrame", printOutFlag_); loadData::loadPtreeValue(pt, armBaseFrame, "model_information.armBaseFrame", printOutFlag_); @@ -125,7 +126,6 @@ MobileManipulatorInterface::MobileManipulatorInterface(const std::string& taskFi std::cout << "\n #### Model Information:" << std::endl; std::cout << "#### =============================================================================" << std::endl; std::cout << "#### model_information.sim: " << sim_ << std::endl; - std::cout << "#### model_information.ns: " << ns_ << std::endl; std::cout << "#### model_information.robotName: " << robotName << std::endl; std::cout << "#### model_information.robotModelType: " << static_cast(robotModelType) << std::endl; std::cout << "#### model_information.removeJoints: " << std::endl; @@ -153,12 +153,12 @@ MobileManipulatorInterface::MobileManipulatorInterface(const std::string& taskFi std::cout << "#### =============================================================================" << std::endl; } - std::cerr << "[MobileManipulatorInterface::MobileManipulatorInterface] START createPinocchioInterface" << std::endl; + //std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] START createPinocchioInterface" << std::endl; // Create pinocchio interface pinocchioInterfacePtr_.reset(new PinocchioInterface(createPinocchioInterface(urdfFile_, robotModelType, removeJointNames, baseFrame, worldFrameName_))); - //std::cerr << *pinocchioInterfacePtr_; + //std::cout << *pinocchioInterfacePtr_; - std::cerr << "[MobileManipulatorInterface::MobileManipulatorInterface] START createRobotModelInfo" << std::endl; + //std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] START createRobotModelInfo" << std::endl; // Set Robot Model Info robotModelInfo_ = createRobotModelInfo(robotName, robotModelType, @@ -171,8 +171,8 @@ MobileManipulatorInterface::MobileManipulatorInterface(const std::string& taskFi // Set Model Settings if (printOutFlag_) { - std::cerr << "\n #### Model Settings:"; - std::cerr << "\n #### =============================================================================\n"; + std::cout << "\n #### Model Settings:"; + std::cout << "\n #### =============================================================================\n"; } loadData::loadPtreeValue(pt, drlFlag_, "model_settings.drlFlag", printOutFlag_); loadData::loadPtreeValue(pt, usePreComputation_, "model_settings.usePreComputation", printOutFlag_); @@ -181,7 +181,7 @@ MobileManipulatorInterface::MobileManipulatorInterface(const std::string& taskFi loadData::loadPtreeValue(pt, activateExtCollision_, "extCollision.activate", printOutFlag_); if (printOutFlag_) { - std::cerr << " #### =============================================================================\n"; + std::cout << " #### =============================================================================\n"; } // Set DDP-MPC settings @@ -192,7 +192,7 @@ MobileManipulatorInterface::MobileManipulatorInterface(const std::string& taskFi referenceManagerPtr_.reset(new ReferenceManager); //// NUA NOTE: Depricated...some initialization that requires node handle is omitted! - std::cerr << "[MobileManipulatorInterface::MobileManipulatorInterface] DEBUG INF" << std::endl; + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] DEBUG INF" << std::endl; while(1); //std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] END" << std::endl; @@ -213,7 +213,7 @@ MobileManipulatorInterface::MobileManipulatorInterface(ros::NodeHandle& nodeHand initModelModeInt_(initModelModeInt), modelModeIntQuery_(initModelModeInt) { - std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface(6)] START" << std::endl; + //std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface(6)] START" << std::endl; mpcTimer0_.reset(); mpcTimer1_.reset(); @@ -242,7 +242,10 @@ MobileManipulatorInterface::MobileManipulatorInterface(ros::NodeHandle& nodeHand boost::filesystem::path taskFilePath(taskFile_); if (boost::filesystem::exists(taskFilePath)) { - std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface(6)] Loading task file: " << taskFilePath << std::endl; + if (printOutFlag_) + { + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface(6)] Loading task file: " << taskFilePath << std::endl; + } } else { @@ -253,7 +256,10 @@ MobileManipulatorInterface::MobileManipulatorInterface(ros::NodeHandle& nodeHand boost::filesystem::path urdfFilePath(urdfFile_); if (boost::filesystem::exists(urdfFilePath)) { - std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface(6)] Loading Pinocchio model from: " << urdfFilePath << std::endl; + if (printOutFlag_) + { + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface(6)] Loading Pinocchio model from: " << urdfFilePath << std::endl; + } } else { @@ -288,7 +294,6 @@ MobileManipulatorInterface::MobileManipulatorInterface(ros::NodeHandle& nodeHand // Read the frame names std::string robotName, baseFrame, armBaseFrame, eeFrame, collisionConstraintPoints, collisionCheckPoints; loadData::loadPtreeValue(pt, sim_, "model_information.sim", printOutFlag_); - loadData::loadPtreeValue(pt, ns_, "model_information.ns", printOutFlag_); loadData::loadPtreeValue(pt, robotName, "model_information.robotName", printOutFlag_); loadData::loadPtreeValue(pt, worldFrameName_, "model_information.worldFrame", printOutFlag_); loadData::loadPtreeValue(pt, baseFrame, "model_information.baseFrame", printOutFlag_); @@ -333,7 +338,6 @@ MobileManipulatorInterface::MobileManipulatorInterface(ros::NodeHandle& nodeHand std::cout << name << std::endl; } std::cout << "#### model_information.sim: " << sim_ << std::endl; - std::cout << "#### model_information.ns: " << ns_ << std::endl; std::cout << "#### model_information.baseFrame: " << baseFrame << std::endl; std::cout << "#### model_information.armBaseFrame: " << armBaseFrame << std::endl; std::cout << "#### model_information.eeFrame: " << eeFrame << std::endl; @@ -355,7 +359,7 @@ MobileManipulatorInterface::MobileManipulatorInterface(ros::NodeHandle& nodeHand } // Adding namespace - std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] ns: " << ns_ << std::endl; + ns_ = nodeHandle.getNamespace(); armJointFrameNames_withNS_ = armJointFrameNames; baseFrame_withNS_ = baseFrame; armBaseFrame_withNS_ = armBaseFrame; @@ -366,19 +370,63 @@ MobileManipulatorInterface::MobileManipulatorInterface(ros::NodeHandle& nodeHand armBaseFrame_withNS_ = ns_ + "/" + armBaseFrame; eeFrame_withNS_ = ns_ + "/" + eeFrame; + if (printOutFlag_) + { + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] ns: " << ns_ << std::endl; + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] armJointFrameNames_withNS_: " << std::endl; + } for (auto& name : armJointFrameNames_withNS_) { name = ns_ + "/" + name; + if (printOutFlag_) + { + std::cout << name << std::endl; + } + } + + armStateMsg_ = ns_ + armStateMsg_; + baseControlMsg_ = ns_ + baseControlMsg_; + armControlMsg_ = ns_ + armControlMsg_; + selfCollisionMsg_ = ns_ + selfCollisionMsg_; + occupancyDistanceBaseMsg_ = ns_ + occupancyDistanceBaseMsg_; + occupancyDistanceArmMsg_ = ns_ + occupancyDistanceArmMsg_; + pointsOnRobotMsgName_ = ns_ + pointsOnRobotMsgName_; + octomapMsg_ = ns_ + octomapMsg_; + modelModeMsgName_ = ns_ + modelModeMsgName_; + mpcTargetMsgName_ = ns_ + mpcTargetMsgName_; + targetMsgName_ = ns_ + targetMsgName_; + goalFrameName_ = ns_ + goalFrameName_; + + if (printOutFlag_) + { + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] baseFrame_withNS_: " << baseFrame_withNS_ << std::endl; + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] armBaseFrame_withNS_: " << armBaseFrame_withNS_ << std::endl; + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] eeFrame_withNS_: " << eeFrame_withNS_ << std::endl; + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] armStateMsg_: " << armStateMsg_ << std::endl; + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] baseControlMsg_: " << baseControlMsg_ << std::endl; + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] armControlMsg_: " << armControlMsg_ << std::endl; + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] selfCollisionMsg_: " << selfCollisionMsg_ << std::endl; + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] occupancyDistanceBaseMsg_: " << occupancyDistanceBaseMsg_ << std::endl; + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] occupancyDistanceArmMsg_: " << occupancyDistanceArmMsg_ << std::endl; + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] pointsOnRobotMsgName_: " << pointsOnRobotMsgName_ << std::endl; + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] octomapMsg_: " << octomapMsg_ << std::endl; + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] modelModeMsgName_: " << modelModeMsgName_ << std::endl; + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] mpcTargetMsgName_: " << mpcTargetMsgName_ << std::endl; + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] targetMsgName_: " << targetMsgName_ << std::endl; + std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] goalFrameName_: " << goalFrameName_ << std::endl; } } + //std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface(6)] DEBUG INF" << std::endl; + //while(1); + // Create pinocchio interface - std::cerr << "[MobileManipulatorInterface::MobileManipulatorInterface] START createPinocchioInterface" << std::endl; - pinocchioInterfacePtr_.reset(new PinocchioInterface(createPinocchioInterface(urdfFile_, robotModelType, removeJointNames, baseFrame_withNS_, worldFrameName_))); - //std::cerr << *pinocchioInterfacePtr_; + //std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] START createPinocchioInterface" << std::endl; + pinocchioInterfacePtr_.reset(new PinocchioInterface(createPinocchioInterface(urdfFile_, robotModelType, removeJointNames, worldFrameName_, baseFrame_withNS_))); + //std::cout << *pinocchioInterfacePtr_; // Set Robot Model Info - std::cerr << "[MobileManipulatorInterface::MobileManipulatorInterface] START createRobotModelInfo" << std::endl; + //std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] START createRobotModelInfo" << std::endl; robotModelInfo_ = createRobotModelInfo(robotName, robotModelType, baseFrame, @@ -390,8 +438,8 @@ MobileManipulatorInterface::MobileManipulatorInterface(ros::NodeHandle& nodeHand // Set Model Settings if (printOutFlag_) { - std::cerr << "\n #### Model Settings:"; - std::cerr << "\n #### =============================================================================\n"; + std::cout << "\n #### Model Settings:"; + std::cout << "\n #### =============================================================================\n"; } loadData::loadPtreeValue(pt, drlFlag_, "model_settings.drlFlag", printOutFlag_); loadData::loadPtreeValue(pt, drlActionType_, "model_settings.drlActionType", printOutFlag_); @@ -401,26 +449,39 @@ MobileManipulatorInterface::MobileManipulatorInterface(ros::NodeHandle& nodeHand loadData::loadPtreeValue(pt, activateExtCollision_, "extCollision.activate", printOutFlag_); if (printOutFlag_) { - std::cerr << " #### =============================================================================\n"; + std::cout << " #### =============================================================================\n"; } // Set DDP-MPC settings ddpSettings_ = ddp::loadSettings(taskFile_, "ddp", printOutFlag_); mpcSettings_ = mpc::loadSettings(taskFile_, "mpc", printOutFlag_); + //std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] START referenceManagerPtr_" << std::endl; // Set Reference Manager referenceManagerPtr_.reset(new ReferenceManager); + //std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] START rosReferenceManagerPtr_" << std::endl; // Set ROS Reference Manager - rosReferenceManagerPtr_ = std::shared_ptr(new ocs2::RosReferenceManager(robotModelName_, referenceManagerPtr_)); + std::string topicPrefix = "mobile_manipulator_"; + if (ns_ != "") + { + topicPrefix = ns_ + "/"; + } + rosReferenceManagerPtr_ = std::shared_ptr(new ocs2::RosReferenceManager(topicPrefix, referenceManagerPtr_)); rosReferenceManagerPtr_->subscribe(nodeHandle_); + //std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] START rolloutSettings_" << std::endl; // Set Rollout Settings rolloutSettings_ = rollout::loadSettings(taskFile_, "rollout", printOutFlag_); + //std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] START initializePointsOnRobotPtr" << std::endl; // Set PointsOnRobot initializePointsOnRobotPtr(collisionConstraintPoints); + //std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface(6)] DEBUG INF" << std::endl; + //while(1); + + //std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] START emuPtr_" << std::endl; // Set ExtMapUtility emuPtr_.reset(new ExtMapUtility()); emuPtr_->setWorldFrameName(worldFrameName_); @@ -433,6 +494,7 @@ MobileManipulatorInterface::MobileManipulatorInterface(ros::NodeHandle& nodeHand bool isModeUpdated; std::string modelName; + //std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] START Mode 0" << std::endl; // Mode 0 modelModeInt = 0; isModeUpdated = updateModelMode(robotModelInfo_, modelModeInt); @@ -451,6 +513,7 @@ MobileManipulatorInterface::MobileManipulatorInterface(ros::NodeHandle& nodeHand recompileLibraries_, printOutFlag_)); + //std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] START Mode 1" << std::endl; // Mode 1 modelModeInt = 1; isModeUpdated = updateModelMode(robotModelInfo_, modelModeInt); @@ -469,6 +532,7 @@ MobileManipulatorInterface::MobileManipulatorInterface(ros::NodeHandle& nodeHand recompileLibraries_, printOutFlag_)); + //std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] START Mode 2" << std::endl; // Mode 2 modelModeInt = 2; isModeUpdated = updateModelMode(robotModelInfo_, modelModeInt); @@ -488,7 +552,7 @@ MobileManipulatorInterface::MobileManipulatorInterface(ros::NodeHandle& nodeHand printOutFlag_)); // Visualization - std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface(6)] BEFORE mobileManipulatorVisu_" << std::endl; + //std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface(6)] BEFORE mobileManipulatorVisu_" << std::endl; mobileManipulatorVisu_.reset(new ocs2::mobile_manipulator::MobileManipulatorVisualization(nodeHandle_, *pinocchioInterfacePtr_, worldFrameName_, @@ -506,14 +570,14 @@ MobileManipulatorInterface::MobileManipulatorInterface(ros::NodeHandle& nodeHand pointsOnRobotPtr_, emuPtr_, maxDistance_)); - std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface(6)] AFTER mobileManipulatorVisu_" << std::endl; + //std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface(6)] AFTER mobileManipulatorVisu_" << std::endl; launchNodes(nodeHandle_); //std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface(6)] DEBUG INF" << std::endl; //while(1); - std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface(6)] END" << std::endl; + //std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface(6)] END" << std::endl; } //------------------------------------------------------------------------------------------------------- @@ -521,10 +585,10 @@ MobileManipulatorInterface::MobileManipulatorInterface(ros::NodeHandle& nodeHand //------------------------------------------------------------------------------------------------------- void MobileManipulatorInterface::initializePointsOnRobotPtr(std::string& collisionPointsName) { - std::cout << "[MobileManipulatorInterface::initializePointsOnRobotPtr] START" << std::endl; + //std::cout << "[MobileManipulatorInterface::initializePointsOnRobotPtr] START" << std::endl; //PointsOnRobot::points_radii_t pointsAndRadii = std::vector>>(); - std::cout << "[MobileManipulatorInterface::initializePointsOnRobotPtr] collisionPointsName: " << collisionPointsName << std::endl; + //std::cout << "[MobileManipulatorInterface::initializePointsOnRobotPtr] collisionPointsName: " << collisionPointsName << std::endl; // Get points on robot parameters PointsOnRobot::points_radii_t pointsAndRadii(8); @@ -536,51 +600,50 @@ void MobileManipulatorInterface::initializePointsOnRobotPtr(std::string& collisi if (collisionPoints.getType() != XmlRpc::XmlRpcValue::TypeArray) { - ROS_WARN("[MobileManipulatorNode::main] collision_points parameter is not of type array."); + ROS_WARN("[MobileManipulatorInterface::initializePointsOnRobotPtr] collision_points parameter is not of type array."); } //// NUA TODO: Get the point and radii info from task file! Also seperate base and arm! - std::cout << "[MobileManipulatorNode::main] pointsAndRadii:" << std::endl; + //std::cout << "[MobileManipulatorInterface::initializePointsOnRobotPtr] pointsAndRadii:" << std::endl; for (int i = 0; i < collisionPoints.size(); i++) { if (collisionPoints.getType() != XmlRpc::XmlRpcValue::TypeArray) { - ROS_WARN_STREAM("[MobileManipulatorNode::main] collision_points[" << i << "] parameter is not of type array."); + ROS_WARN_STREAM("[MobileManipulatorInterface::initializePointsOnRobotPtr] collision_points[" << i << "] parameter is not of type array."); } for (int j = 0; j < collisionPoints[i].size(); j++) { if (collisionPoints[j].getType() != XmlRpc::XmlRpcValue::TypeArray) { - ROS_WARN_STREAM("[MobileManipulatorNode::main] collision_points[" << i << "][" << j << "] parameter is not of type array."); + ROS_WARN_STREAM("[MobileManipulatorInterface::initializePointsOnRobotPtr] collision_points[" << i << "][" << j << "] parameter is not of type array."); } if (collisionPoints[i][j].size() != 2) { - ROS_WARN_STREAM("[MobileManipulatorNode::main] collision_points[" << i << "][" << j << "] does not have 2 elements."); + ROS_WARN_STREAM("[MobileManipulatorInterface::initializePointsOnRobotPtr] collision_points[" << i << "][" << j << "] does not have 2 elements."); } double segmentId = collisionPoints[i][j][0]; double radius = collisionPoints[i][j][1]; pointsAndRadii[i].push_back(pair_t(segmentId, radius)); - ROS_INFO_STREAM("[MobileManipulatorNode::main] segment=" << i << ". relative pos on segment:" << segmentId << ". radius:" << radius); + //std::cout << "[MobileManipulatorInterface::initializePointsOnRobotPtr] segment=" << i << ". relative pos on segment:" << segmentId << ". radius:" << radius << std::endl; } } } else { - std::cout << "[MobileManipulatorNode::main] ERROR: collision_points is not defined!" << std::endl; + std::cout << "[MobileManipulatorInterface::initializePointsOnRobotPtr] ERROR: collision_points is not defined!" << std::endl; } pointsOnRobotPtr_.reset(new PointsOnRobot(pointsAndRadii)); if (pointsOnRobotPtr_->getNumOfPoints() > 0) { - std::cout << "[MobileManipulatorNode::main] BEFORE initialize pointsOnRobotMsgName_: " << pointsOnRobotMsgName_ << std::endl; pointsOnRobotPtr_->initialize(*pinocchioInterfacePtr_, MobileManipulatorPinocchioMapping(robotModelInfo_), MobileManipulatorPinocchioMappingCppAd(robotModelInfo_), robotModelInfo_, - pointsOnRobotMsgName_, + "points_on_robot", libraryFolder_, recompileLibraries_, false); @@ -590,7 +653,9 @@ void MobileManipulatorInterface::initializePointsOnRobotPtr(std::string& collisi pointsOnRobotPtr_ = nullptr; } - std::cout << "[MobileManipulatorInterface::initializePointsOnRobotPtr] END" << std::endl; + + + //std::cout << "[MobileManipulatorInterface::initializePointsOnRobotPtr] END" << std::endl; } //------------------------------------------------------------------------------------------------------- @@ -693,7 +758,7 @@ void MobileManipulatorInterface::setMPCProblem() mpcTimer8_.endTimer(); mpcTimer9_.startTimer(); - std::cout << "[MobileManipulatorInterface::setMPCProblem] dynamicsPtr: MobileBaseDynamics" << std::endl; + //std::cout << "[MobileManipulatorInterface::setMPCProblem] dynamicsPtr: MobileBaseDynamics" << std::endl; ocp_.dynamicsPtr = dynamicsPtr_mode0_; mpcTimer9_.endTimer(); } @@ -731,7 +796,7 @@ void MobileManipulatorInterface::setMPCProblem() mpcTimer8_.endTimer(); mpcTimer9_.startTimer(); - std::cout << "[MobileManipulatorInterface::setMPCProblem] dynamicsPtr: RobotArmDynamics" << std::endl; + //std::cout << "[MobileManipulatorInterface::setMPCProblem] dynamicsPtr: RobotArmDynamics" << std::endl; ocp_.dynamicsPtr = dynamicsPtr_mode1_; mpcTimer9_.endTimer(); } @@ -764,13 +829,13 @@ void MobileManipulatorInterface::setMPCProblem() mpcTimer8_.startTimer(); if (activateExtCollision_) { - std::cout << "[MobileManipulatorInterface::setMPCProblem] BEFORE extCollisionConstraintPtr_mode2_" << std::endl; + //std::cout << "[MobileManipulatorInterface::setMPCProblem] BEFORE extCollisionConstraintPtr_mode2_" << std::endl; ocp_.stateSoftConstraintPtr->add("extCollision", extCollisionConstraintPtr_mode2_); } mpcTimer8_.endTimer(); mpcTimer9_.startTimer(); - std::cout << "[MobileManipulatorInterface::setMPCProblem] dynamicsPtr: MobileManipulatorDynamics" << std::endl; + //std::cout << "[MobileManipulatorInterface::setMPCProblem] dynamicsPtr: MobileManipulatorDynamics" << std::endl; ocp_.dynamicsPtr = dynamicsPtr_mode2_; mpcTimer9_.endTimer(); } @@ -857,7 +922,7 @@ void MobileManipulatorInterface::setMPCProblem() //std::cout << "[MobileManipulatorInterface::MobileManipulatorInterface] DEBUG INF" << std::endl; //while(1); - std::cout << "[MobileManipulatorInterface::setMPCProblem] END" << std::endl; + //std::cout << "[MobileManipulatorInterface::setMPCProblem] END" << std::endl; } //------------------------------------------------------------------------------------------------------- @@ -869,12 +934,12 @@ void MobileManipulatorInterface::launchNodes(ros::NodeHandle& nodeHandle) { if (drlActionType_ == 0) { - std::cout << "[MobileManipulatorInterface::launchNodes] DISCRETE ACTION" << std::endl; + //std::cout << "[MobileManipulatorInterface::launchNodes] DISCRETE ACTION" << std::endl; setActionDRLService_ = nodeHandle_.advertiseService("set_action_drl", &MobileManipulatorInterface::setDiscreteActionDRLSrv, this); } else { - std::cout << "[MobileManipulatorInterface::launchNodes] CONTINUOUS ACTION" << std::endl; + //std::cout << "[MobileManipulatorInterface::launchNodes] CONTINUOUS ACTION" << std::endl; setActionDRLService_ = nodeHandle_.advertiseService("set_action_drl", &MobileManipulatorInterface::setContinuousActionDRLSrv, this); } @@ -887,14 +952,14 @@ void MobileManipulatorInterface::launchNodes(ros::NodeHandle& nodeHandle) // Subscribe Model Mode auto modelModeCallback = [this](const std_msgs::UInt8::ConstPtr& msg) { - std::cout << "[MobileManipulatorInterface::launchNodes::modelModeCallback] START" << std::endl; + //std::cout << "[MobileManipulatorInterface::launchNodes::modelModeCallback] START" << std::endl; // Shutdown MRT - std::cout << "[MobileManipulatorInterface::launchNodes::modelModeCallback] mrtShutDownFlag true" << std::endl; + //std::cout << "[MobileManipulatorInterface::launchNodes::modelModeCallback] mrtShutDownFlag true" << std::endl; mrtShutDownEnvStatus_ = setenv("mrtShutDownFlag", "true", 1); modelModeIntQuery_ = msg->data; - std::cout << "[MobileManipulatorInterface::launchNodes::modelModeCallback] modelModeIntQuery_: " << modelModeIntQuery_ << std::endl; + //std::cout << "[MobileManipulatorInterface::launchNodes::modelModeCallback] modelModeIntQuery_: " << modelModeIntQuery_ << std::endl; //mpcTimer3_.startTimer(); //setMPCProblem(); @@ -909,8 +974,8 @@ void MobileManipulatorInterface::launchNodes(ros::NodeHandle& nodeHandle) //std::cout << "\n### Average : " << mpcTimer3_.getAverageInMilliseconds() << "[ms]."; //std::cout << "\n### Latest : " << mpcTimer3_.getLastIntervalInMilliseconds() << "[ms]." << std::endl; - std::cout << "[MobileManipulatorInterface::launchNodes::modelModeCallback] END" << std::endl; - std::cout << "" << std::endl; + //std::cout << "[MobileManipulatorInterface::launchNodes::modelModeCallback] END" << std::endl; + //std::cout << "" << std::endl; }; modelModeSubscriber_ = nodeHandle_.subscribe(modelModeMsgName_, 1, modelModeCallback); } @@ -929,7 +994,7 @@ void MobileManipulatorInterface::launchNodes(ros::NodeHandle& nodeHandle) } else { - std::cout << "[MobileManipulatorInterface::targetTrajectoriesCallback] ERROR: Size mismatch!" << std::endl; + //std::cout << "[MobileManipulatorInterface::targetTrajectoriesCallback] ERROR: Size mismatch!" << std::endl; } /* @@ -1022,7 +1087,7 @@ void MobileManipulatorInterface::getGraspPose(vector_t& targetPose) bool MobileManipulatorInterface::setDiscreteActionDRLSrv(ocs2_msgs::setDiscreteActionDRL::Request &req, ocs2_msgs::setDiscreteActionDRL::Response &res) { - std::cout << "[MobileManipulatorInterface::setDiscreteActionDRLSrv] START" << std::endl; + //std::cout << "[MobileManipulatorInterface::setDiscreteActionDRLSrv] START" << std::endl; drlActionDiscrete_ = req.action; drlActionTimeHorizon_ = req.time_horizon; @@ -1046,7 +1111,7 @@ bool MobileManipulatorInterface::setDiscreteActionDRLSrv(ocs2_msgs::setDiscreteA std::cout << "[MobileManipulatorInterface::setDiscreteActionDRLSrv] DEBUG INF" << std::endl; while(1); - std::cout << "[MobileManipulatorInterface::setDiscreteActionDRLSrv] END" << std::endl; + //std::cout << "[MobileManipulatorInterface::setDiscreteActionDRLSrv] END" << std::endl; return res.success; } @@ -1056,7 +1121,7 @@ bool MobileManipulatorInterface::setDiscreteActionDRLSrv(ocs2_msgs::setDiscreteA bool MobileManipulatorInterface::setContinuousActionDRLSrv(ocs2_msgs::setContinuousActionDRL::Request &req, ocs2_msgs::setContinuousActionDRL::Response &res) { - std::cout << "[MobileManipulatorInterface::setContinuousActionDRLSrv] START" << std::endl; + //std::cout << "[MobileManipulatorInterface::setContinuousActionDRLSrv] START" << std::endl; drlActionContinuous_ = req.action; drlActionTimeHorizon_ = req.time_horizon; drlActionLastStepFlag_ = req.last_step_flag; @@ -1087,7 +1152,7 @@ bool MobileManipulatorInterface::setContinuousActionDRLSrv(ocs2_msgs::setContinu //std::cout << "[MobileManipulatorInterface::setContinuousActionDRLSrv] DEBUG INF" << std::endl; //while(1); - std::cout << "[MobileManipulatorInterface::setContinuousActionDRLSrv] END" << std::endl; + //std::cout << "[MobileManipulatorInterface::setContinuousActionDRLSrv] END" << std::endl; return res.success; } @@ -1101,13 +1166,22 @@ void MobileManipulatorInterface::runMPC() bool mpcPrintOutFlag = false; RobotModelInfo robotModelInfo; + std::string mpcTopicPrefix = "mobile_manipulator_"; + if (ns_ != "") + { + mpcTopicPrefix = ns_ + "/"; + } mpcIter_ = 0; while (ros::ok() && ros::master::check()) { - std::cout << "===================== MPC START =====================" << std::endl; - std::cout << "[MobileManipulatorInterface::runMPC] mpcIter_: " << mpcIter_ << std::endl; - std::cout << "[MobileManipulatorInterface::runMPC] mrtIter_: " << mrtIter_ << std::endl; + if (mpcPrintOutFlag) + { + std::cout << "===================== MPC START =====================" << std::endl; + std::cout << "[MobileManipulatorInterface::runMPC] START ITERATION: " << mpcIter_ << std::endl; + std::cout << "[MobileManipulatorInterface::runMPC] mpcIter_: " << mpcIter_ << std::endl; + std::cout << "[MobileManipulatorInterface::runMPC] mrtIter_: " << mrtIter_ << std::endl; + } /* if (mpcIter_ > 0) @@ -1120,15 +1194,6 @@ void MobileManipulatorInterface::runMPC() // Wait for sync mpc and mrt while(mpcIter_ != mrtIter_){spinOnce();} - if (mpcPrintOutFlag) - { - std::cout << "===================== MPC START =====================" << std::endl; - std::cout << "=====================================================" << std::endl; - std::cout << "=====================================================" << std::endl; - std::cout << "=====================================================" << std::endl; - std::cout << "[MobileManipulatorInterface::runMPC] START ITERATION: " << mpcIter_ << std::endl; - } - //mpcTimer1_.startTimer(); // Robot interface @@ -1155,7 +1220,7 @@ void MobileManipulatorInterface::runMPC() if (drlFlag_) { - std::cout << "[MobileManipulatorInterface::runMPC] START DRL TRAINING!!!" << std::endl; + //std::cout << "[MobileManipulatorInterface::runMPC] START DRL TRAINING!!!" << std::endl; while(!targetReceivedFlag_) { setMRTReady(); @@ -1218,21 +1283,22 @@ void MobileManipulatorInterface::runMPC() // Launch MPC ROS node //std::cout << "[MobileManipulatorInterface::runMPC] BEFORE mpc mpcNode" << std::endl; //mpcTimer8_.startTimer(); - MPC_ROS_Interface mpcNode(mpc, robotModelName_); + MPC_ROS_Interface mpcNode(mpc, mpcTopicPrefix); mpcNode.setModelModeInt(getModelModeInt(robotModelInfo)); //mpcTimer8_.endTimer(); //mpcTimer1_.endTimer(); - if (false) + if (printOutFlag_) { + /* std::cout << '\n'; std::cout << "\n### MPC_ROS Benchmarking mpcTimer1_"; std::cout << "\n### Maximum : " << mpcTimer1_.getMaxIntervalInMilliseconds() << "[ms]."; std::cout << "\n### Average : " << mpcTimer1_.getAverageInMilliseconds() << "[ms]."; std::cout << "\n### Latest : " << mpcTimer1_.getLastIntervalInMilliseconds() << "[ms]." << std::endl; - /* + std::cout << "\n### MPC_ROS Benchmarking mpcTimer2_"; std::cout << "\n### Maximum : " << mpcTimer2_.getMaxIntervalInMilliseconds() << "[ms]."; std::cout << "\n### Average : " << mpcTimer2_.getAverageInMilliseconds() << "[ms]."; @@ -1241,7 +1307,7 @@ void MobileManipulatorInterface::runMPC() std::cout << "\n### Maximum : " << mpcTimer3_.getMaxIntervalInMilliseconds() << "[ms]."; std::cout << "\n### Average : " << mpcTimer3_.getAverageInMilliseconds() << "[ms]."; std::cout << "\n### Latest : " << mpcTimer3_.getLastIntervalInMilliseconds() << "[ms]." << std::endl; - */ + std::cout << "\n### MPC_ROS Benchmarking mpcTimer4_"; std::cout << "\n### Maximum : " << mpcTimer4_.getMaxIntervalInMilliseconds() << "[ms]."; std::cout << "\n### Average : " << mpcTimer4_.getAverageInMilliseconds() << "[ms]."; @@ -1262,6 +1328,7 @@ void MobileManipulatorInterface::runMPC() std::cout << "\n### Maximum : " << mpcTimer8_.getMaxIntervalInMilliseconds() << "[ms]."; std::cout << "\n### Average : " << mpcTimer8_.getAverageInMilliseconds() << "[ms]."; std::cout << "\n### Latest : " << mpcTimer8_.getLastIntervalInMilliseconds() << "[ms]." << std::endl; + */ } //std::cout << "[MobileManipulatorInterface::runMPC] BEFORE mpcLaunchReadyFlag_: " << mpcLaunchReadyFlag_ << std::endl; @@ -1288,7 +1355,7 @@ void MobileManipulatorInterface::runMPC() mpcIter_++; - std::cout << "====================== MPC END ======================" << std::endl; + //std::cout << "====================== MPC END ======================" << std::endl; } //std::cout << "[MobileManipulatorInterface::runMPC] END" << std::endl; @@ -1304,16 +1371,23 @@ void MobileManipulatorInterface::runMRT() bool mrtPrintOutFlag = false; RobotModelInfo robotModelInfo; - //OptimalControlProblem ocp; + std::string mrtTopicPrefix = "mobile_manipulator_"; + if (ns_ != "") + { + mrtTopicPrefix = ns_ + "/"; + } getEEPose(currentTarget_); mrtIter_ = 0; while (ros::ok() && ros::master::check()) { - std::cout << "*********************** MRT START *******************" << std::endl; - std::cout << "[MobileManipulatorInterface::runMRT] mpcIter_: " << mpcIter_ << std::endl; - std::cout << "[MobileManipulatorInterface::runMRT] mrtIter_: " << mrtIter_ << std::endl; + if (printOutFlag_) + { + std::cout << "*********************** MRT START *******************" << std::endl; + std::cout << "[MobileManipulatorInterface::runMRT] mpcIter_: " << mpcIter_ << std::endl; + std::cout << "[MobileManipulatorInterface::runMRT] mrtIter_: " << mrtIter_ << std::endl; + } // Wait for sync mpc and mrt while(mpcIter_ != mrtIter_){spinOnce();} @@ -1352,7 +1426,7 @@ void MobileManipulatorInterface::runMRT() // MRT //std::cout << "[MobileManipulatorInterface::runMRT] BEFORE mrt" << std::endl; //mrtTimer3_.startTimer(); - MRT_ROS_Interface mrt(robotModelInfo, robotModelName_); + MRT_ROS_Interface mrt(robotModelInfo, mrtTopicPrefix); //mrtTimer3_.endTimer(); //std::cout << "[MobileManipulatorInterface::runMRT] BEFORE initRollout" << std::endl; @@ -1429,8 +1503,9 @@ void MobileManipulatorInterface::runMRT() //std::cout << "[MobileManipulatorInterface::runMRT] BEFORE run" << std::endl; //mrtTimer1_.endTimer(); - if (false) + if (printOutFlag_) { + /* std::cout << '\n'; std::cout << "\n### MRT_ROS Benchmarking mrtTimer1_"; std::cout << "\n### Maximum : " << mrtTimer1_.getMaxIntervalInMilliseconds() << "[ms]."; @@ -1460,6 +1535,7 @@ void MobileManipulatorInterface::runMRT() std::cout << "\n### Maximum : " << mrtTimer7_.getMaxIntervalInMilliseconds() << "[ms]."; std::cout << "\n### Average : " << mrtTimer7_.getAverageInMilliseconds() << "[ms]."; std::cout << "\n### Latest : " << mrtTimer7_.getLastIntervalInMilliseconds() << "[ms]." << std::endl; + */ } setenv("mrtExitFlag", "false", 1); @@ -1488,19 +1564,17 @@ void MobileManipulatorInterface::runMRT() } */ + //std::cout << "[MobileManipulatorInterface::runMRT] DEBUG INF" << std::endl; + //while(1); + + mrtIter_++; + if (mrtPrintOutFlag) { std::cout << "[MobileManipulatorInterface::runMRT] END ITERATION: " << mrtIter_ << std::endl; std::cout << "*****************************************************" << std::endl; std::cout << "********************* MRT END ***********************" << std::endl; } - - //std::cout << "[MobileManipulatorInterface::runMRT] DEBUG INF" << std::endl; - //while(1); - - mrtIter_++; - - std::cout << "********************* MRT END ***********************" << std::endl; } //std::cout << "[MobileManipulatorInterface::runMRT] END" << std::endl; @@ -1578,10 +1652,10 @@ std::unique_ptr MobileManipulatorInterface::getQuadraticInputCos if (printOutFlag_) { - std::cerr << "\n #### Input Cost Settings: "; - std::cerr << "\n #### =============================================================================\n"; - std::cerr << "inputCost.R: \n" << R << '\n'; - std::cerr << " #### =============================================================================\n"; + std::cout << "\n #### Input Cost Settings: "; + std::cout << "\n #### =============================================================================\n"; + std::cout << "inputCost.R: \n" << R << '\n'; + std::cout << " #### =============================================================================\n"; } return std::unique_ptr(new QuadraticInputCost(std::move(R), modeStateDim)); @@ -1626,13 +1700,13 @@ std::unique_ptr MobileManipulatorInterface::getJointLimitSoftCon scalar_t muPositionLimits = 1e-2; scalar_t deltaPositionLimits = 1e-3; - //std::cerr << "\n #### ArmJointPositionLimits Settings: "; - //std::cerr << "\n #### =============================================================================\n"; - //std::cerr << " #### lowerBound: " << lowerBound.transpose() << '\n'; - //std::cerr << " #### upperBound: " << upperBound.transpose() << '\n'; + //std::cout << "\n #### ArmJointPositionLimits Settings: "; + //std::cout << "\n #### =============================================================================\n"; + //std::cout << " #### lowerBound: " << lowerBound.transpose() << '\n'; + //std::cout << " #### upperBound: " << upperBound.transpose() << '\n'; loadData::loadPtreeValue(pt, muPositionLimits, "jointPositionLimits.mu", printOutFlag_); loadData::loadPtreeValue(pt, deltaPositionLimits, "jointPositionLimits.delta", printOutFlag_); - //std::cerr << " #### =============================================================================\n"; + //std::cout << " #### =============================================================================\n"; stateLimits.reserve(modeStateDim); const size_t stateOffset = modeStateDim - stateDimArm; @@ -1695,13 +1769,13 @@ std::unique_ptr MobileManipulatorInterface::getJointLimitSoftCon scalar_t muVelocityLimits = 1e-2; scalar_t deltaVelocityLimits = 1e-3; - //std::cerr << "\n #### JointVelocityLimits Settings: "; - //std::cerr << "\n #### =============================================================================\n"; - //std::cerr << " #### 'lowerBound': " << lowerBound.transpose() << std::endl; - //std::cerr << " #### 'upperBound': " << upperBound.transpose() << std::endl; + //std::cout << "\n #### JointVelocityLimits Settings: "; + //std::cout << "\n #### =============================================================================\n"; + //std::cout << " #### 'lowerBound': " << lowerBound.transpose() << std::endl; + //std::cout << " #### 'upperBound': " << upperBound.transpose() << std::endl; loadData::loadPtreeValue(pt, muVelocityLimits, "jointVelocityLimits.mu", printOutFlag_); loadData::loadPtreeValue(pt, deltaVelocityLimits, "jointVelocityLimits.delta", printOutFlag_); - //std::cerr << " #### =============================================================================\n"; + //std::cout << " #### =============================================================================\n"; inputLimits.reserve(modeInputDim); for (int i = 0; i < modeInputDim; ++i) @@ -1750,11 +1824,11 @@ std::unique_ptr MobileManipulatorInterface::getEndEffectorConstraint( std::string modelName = getModelModeString(robotModelInfo_) + "_end_effector_kinematics"; scalar_t muPosition; scalar_t muOrientation; - //std::cerr << "\n #### " << prefix << " Settings: "; - //std::cerr << "\n #### =============================================================================\n"; + //std::cout << "\n #### " << prefix << " Settings: "; + //std::cout << "\n #### =============================================================================\n"; loadData::loadPtreeValue(pt, muPosition, prefix + ".muPosition", printOutFlag_); loadData::loadPtreeValue(pt, muOrientation, prefix + ".muOrientation", printOutFlag_); - //std::cerr << " #### =============================================================================\n"; + //std::cout << " #### =============================================================================\n"; if (referenceManagerPtr_ == nullptr) { @@ -1816,18 +1890,18 @@ std::unique_ptr MobileManipulatorInterface::getSelfCollisionConstrain //std::vector> collisionObjectPairs; //std::vector> collisionLinkPairs; - //std::cerr << "\n #### SelfCollision Settings: "; - //std::cerr << "\n #### =============================================================================\n"; + //std::cout << "\n #### SelfCollision Settings: "; + //std::cout << "\n #### =============================================================================\n"; loadData::loadPtreeValue(pt, mu, "selfCollision.mu", printOutFlag_); loadData::loadPtreeValue(pt, delta, "selfCollision.delta", printOutFlag_); loadData::loadPtreeValue(pt, minimumDistance, "selfCollision.minimumDistance", printOutFlag_); loadData::loadStdVectorOfPair(taskFile_, "selfCollision.collisionObjectPairs", collisionObjectPairs_, printOutFlag_); loadData::loadStdVectorOfPair(taskFile_, "selfCollision.collisionLinkPairs", collisionLinkPairs_, printOutFlag_); - //std::cerr << " #### =============================================================================\n"; + //std::cout << " #### =============================================================================\n"; PinocchioGeometryInterface geometryInterface(*pinocchioInterfacePtr_, collisionLinkPairs_, collisionObjectPairs_); const size_t numCollisionPairs = geometryInterface.getNumCollisionPairs(); - //std::cerr << "[MobileManipulatorInterface::getSelfCollisionConstraint] Testing for " << numCollisionPairs << " collision pairs\n"; + //std::cout << "[MobileManipulatorInterface::getSelfCollisionConstraint] Testing for " << numCollisionPairs << " collision pairs\n"; std::unique_ptr constraint; @@ -1866,7 +1940,7 @@ std::unique_ptr MobileManipulatorInterface::getSelfCollisionConstrain //------------------------------------------------------------------------------------------------------- std::unique_ptr MobileManipulatorInterface::getExtCollisionConstraint(const std::string& prefix) { - std::cout << "[MobileManipulatorInterface::getExtCollisionConstraint] START" << std::endl; + //std::cout << "[MobileManipulatorInterface::getExtCollisionConstraint] START" << std::endl; boost::property_tree::ptree pt; boost::property_tree::read_info(taskFile_, pt); @@ -1877,12 +1951,12 @@ std::unique_ptr MobileManipulatorInterface::getExtCollisionConstraint scalar_t mu = 1e-2; scalar_t delta = 1e-3; //scalar_t maxDistance = 10; - std::cerr << "\n #### ExtCollision Settings: "; - std::cerr << "\n #### =============================================================================\n"; - loadData::loadPtreeValue(pt, mu, prefix + ".mu", true); - loadData::loadPtreeValue(pt, delta, prefix + ".delta", true); - loadData::loadPtreeValue(pt, maxDistance_, prefix + ".maxDistance", true); - std::cerr << " #### =============================================================================\n"; + //std::cout << "\n #### ExtCollision Settings: "; + //std::cout << "\n #### =============================================================================\n"; + loadData::loadPtreeValue(pt, mu, prefix + ".mu", printOutFlag_); + loadData::loadPtreeValue(pt, delta, prefix + ".delta", printOutFlag_); + loadData::loadPtreeValue(pt, maxDistance_, prefix + ".maxDistance", printOutFlag_); + //std::cout << " #### =============================================================================\n"; ExtCollisionPinocchioGeometryInterface extCollisionPinocchioGeometryInterface(*pinocchioInterfacePtr_); @@ -1906,7 +1980,7 @@ std::unique_ptr MobileManipulatorInterface::getExtCollisionConstraint } else { - std::cout << "[MobileManipulatorInterface::getExtCollisionConstraint] usePreComputation_ false" << std::endl; + //std::cout << "[MobileManipulatorInterface::getExtCollisionConstraint] usePreComputation_ false" << std::endl; constraint = std::unique_ptr(new ExtCollisionConstraintCppAd(*pinocchioInterfacePtr_, MobileManipulatorPinocchioMapping(robotModelInfo_), @@ -1922,7 +1996,7 @@ std::unique_ptr MobileManipulatorInterface::getExtCollisionConstraint std::unique_ptr penalty(new RelaxedBarrierPenalty({mu, delta})); - std::cout << "[MobileManipulatorInterface::getExtCollisionConstraint] END" << std::endl; + //std::cout << "[MobileManipulatorInterface::getExtCollisionConstraint] END" << std::endl; return std::unique_ptr(new StateSoftConstraint(std::move(constraint), std::move(penalty))); } @@ -1932,7 +2006,7 @@ std::unique_ptr MobileManipulatorInterface::getExtCollisionConstraint //------------------------------------------------------------------------------------------------------- bool MobileManipulatorInterface::setTargetDRL(double x, double y, double z, double roll, double pitch, double yaw) { - std::cout << "[MobileManipulatorInterface::setTargetDRL] START" << std::endl; + //std::cout << "[MobileManipulatorInterface::setTargetDRL] START" << std::endl; bool success = false; ocs2_msgs::setTask srv; @@ -1959,7 +2033,7 @@ bool MobileManipulatorInterface::setTargetDRL(double x, double y, double z, doub success = false; } - std::cout << "[MobileManipulatorInterface::setTargetDRL] END" << std::endl; + //std::cout << "[MobileManipulatorInterface::setTargetDRL] END" << std::endl; return success; } @@ -1969,9 +2043,11 @@ bool MobileManipulatorInterface::setTargetDRL(double x, double y, double z, doub //------------------------------------------------------------------------------------------------------- void MobileManipulatorInterface::mapDiscreteActionDRL(int action) { - std::cout << "[MobileManipulatorInterface::mapDiscreteActionDRL] START" << std::endl; + //std::cout << "[MobileManipulatorInterface::mapDiscreteActionDRL] START" << std::endl; /// NUA TODO: NEEDS DEBUGGING! + std::cout << "[MobileManipulatorInterface::mapDiscreteActionDRL] DEBUG_INF" << std::endl; + while(1); // MPC Settings Variables ------- int n_mode = 3; @@ -1996,10 +2072,10 @@ void MobileManipulatorInterface::mapDiscreteActionDRL(int action) { tmp_name = mpcProblemSettings_.binarySettingNames[i]; tmp_val = mpcProblemSettings_.binarySettingValues[i]; - std::cout << "[MobileManipulatorInterface::mapDiscreteActionDRL] " << tmp_name << ": " << tmp_val << std::endl; + //std::cout << "[MobileManipulatorInterface::mapDiscreteActionDRL] " << tmp_name << ": " << tmp_val << std::endl; } - std::cout << "[MobileManipulatorInterface::mapDiscreteActionDRL] END" << std::endl; + //std::cout << "[MobileManipulatorInterface::mapDiscreteActionDRL] END" << std::endl; } //------------------------------------------------------------------------------------------------------- @@ -2007,7 +2083,7 @@ void MobileManipulatorInterface::mapDiscreteActionDRL(int action) //------------------------------------------------------------------------------------------------------- void MobileManipulatorInterface::mapContinuousActionDRL(std::vector& action) { - std::cout << "[MobileManipulatorInterface::mapContinuousActionDRL] START" << std::endl; + //std::cout << "[MobileManipulatorInterface::mapContinuousActionDRL] START" << std::endl; double modelModeProb = action[0]; double constraintProb = action[1]; @@ -2021,7 +2097,7 @@ void MobileManipulatorInterface::mapContinuousActionDRL(std::vector& act // Set Model Mode if (modelModeProb <= 0.3) { - std::cout << "[MobileManipulatorInterface::mapContinuousActionDRL] BASE MOTION" << std::endl; + //std::cout << "[MobileManipulatorInterface::mapContinuousActionDRL] BASE MOTION" << std::endl; target_roll = 0.0; target_pitch = 0.0; target_z = 0.12; @@ -2029,12 +2105,12 @@ void MobileManipulatorInterface::mapContinuousActionDRL(std::vector& act } else if (modelModeProb > 0.6) { - std::cout << "[MobileManipulatorInterface::mapContinuousActionDRL] WHOLE-BODY MOTION" << std::endl; + //std::cout << "[MobileManipulatorInterface::mapContinuousActionDRL] WHOLE-BODY MOTION" << std::endl; modelModeIntQuery_ = 2; } else { - std::cout << "[MobileManipulatorInterface::mapContinuousActionDRL] ARM MOTION" << std::endl; + //std::cout << "[MobileManipulatorInterface::mapContinuousActionDRL] ARM MOTION" << std::endl; modelModeIntQuery_ = 1; } @@ -2051,7 +2127,7 @@ void MobileManipulatorInterface::mapContinuousActionDRL(std::vector& act // Set Target setTargetDRL(target_x, target_y, target_z, target_roll, target_pitch, target_yaw); - std::cout << "[MobileManipulatorInterface::mapContinuousActionDRL] END" << std::endl; + //std::cout << "[MobileManipulatorInterface::mapContinuousActionDRL] END" << std::endl; } //------------------------------------------------------------------------------------------------------- @@ -2086,7 +2162,7 @@ bool MobileManipulatorInterface::setMRTReady() //------------------------------------------------------------------------------------------------------- bool MobileManipulatorInterface::setMPCActionResult(int drlActionResult) { - std::cout << "[MobileManipulatorInterface::setMPCActionResult] START" << std::endl; + //std::cout << "[MobileManipulatorInterface::setMPCActionResult] START" << std::endl; bool success = false; ocs2_msgs::setMPCActionResult srv; @@ -2103,7 +2179,7 @@ bool MobileManipulatorInterface::setMPCActionResult(int drlActionResult) success = false; } - std::cout << "[MobileManipulatorInterface::setMPCActionResult] END" << std::endl; + //std::cout << "[MobileManipulatorInterface::setMPCActionResult] END" << std::endl; return success; } diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/src/MobileManipulatorVisualization.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator/src/MobileManipulatorVisualization.cpp index dccc9f91a..646a310e1 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/src/MobileManipulatorVisualization.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/src/MobileManipulatorVisualization.cpp @@ -104,18 +104,18 @@ MobileManipulatorVisualization::MobileManipulatorVisualization(ros::NodeHandle& pinocchioInterfaceInternal_(pinocchioInterface) //distances_(pointsOnRobotPtr->getNumOfPoints()) { - std::cout << "[MobileManipulatorVisualization::MobileManipulatorVisualization] START" << std::endl; + //std::cout << "[MobileManipulatorVisualization::MobileManipulatorVisualization] START" << std::endl; - std::cout << "[MobileManipulatorVisualization::MobileManipulatorVisualization] ns: " << ns << std::endl; - std::cout << "[MobileManipulatorVisualization::MobileManipulatorVisualization] baseFrameName: " << baseFrameName << std::endl; + //std::cout << "[MobileManipulatorVisualization::MobileManipulatorVisualization] ns: " << ns << std::endl; + //std::cout << "[MobileManipulatorVisualization::MobileManipulatorVisualization] baseFrameName: " << baseFrameName << std::endl; if (ns != "") { baseFrameName_withNS_ = ns + "/" + baseFrameName; } - std::cout << "[MobileManipulatorVisualization::MobileManipulatorVisualization] baseFrameName_withNS_: " << baseFrameName_withNS_ << std::endl; + //std::cout << "[MobileManipulatorVisualization::MobileManipulatorVisualization] baseFrameName_withNS_: " << baseFrameName_withNS_ << std::endl; launchVisualizerNode(nodeHandle); - std::cout << "[MobileManipulatorVisualization::MobileManipulatorVisualization] END" << std::endl; + //std::cout << "[MobileManipulatorVisualization::MobileManipulatorVisualization] END" << std::endl; } //------------------------------------------------------------------------------------------------------- @@ -131,21 +131,21 @@ void MobileManipulatorVisualization::setObjOctomapNames(std::vector //------------------------------------------------------------------------------------------------------- void MobileManipulatorVisualization::launchVisualizerNode(ros::NodeHandle& nodeHandle) { - std::cout << "[MobileManipulatorVisualization::launchVisualizerNode] START" << std::endl; + //std::cout << "[MobileManipulatorVisualization::launchVisualizerNode] START" << std::endl; // Subscribers jointStatesSub_ = nodeHandle.subscribe(jointStateMsgName_, 5, &MobileManipulatorVisualization::jointStateCallback, this); tfSub_ = nodeHandle.subscribe("/tf", 5, &MobileManipulatorVisualization::tfCallback, this); // Publishers - stateOptimizedPublisher_ = nodeHandle.advertise("/mobile_manipulator/optimizedStateTrajectory", 1); - stateOptimizedPosePublisher_ = nodeHandle.advertise("/mobile_manipulator/optimizedPoseTrajectory", 1); + stateOptimizedPublisher_ = nodeHandle.advertise(ns_ + "/optimizedStateTrajectory", 1); + stateOptimizedPosePublisher_ = nodeHandle.advertise(ns_ + "/optimizedPoseTrajectory", 1); pubSelfCollisionInfo_ = nodeHandle.advertise(selfCollisionMsg_, 10, true); markerPublisher_ = nodeHandle.advertise(selfCollisionMsg_ + "_visu", 10, true); // Create pinocchio interface - pinocchioInterfaceInternal_ = mobile_manipulator::createPinocchioInterface(urdfFile_, robotModelInfo_.robotModelType, removeJointNames_, baseFrameName_withNS_, worldFrameName_); + pinocchioInterfaceInternal_ = mobile_manipulator::createPinocchioInterface(urdfFile_, robotModelInfo_.robotModelType, removeJointNames_, worldFrameName_, baseFrameName_withNS_); // activate markers for self-collision visualization if (selfCollisionFlag_) @@ -158,11 +158,11 @@ void MobileManipulatorVisualization::launchVisualizerNode(ros::NodeHandle& nodeH visualizationInterfacePtr_.reset(new GeometryInterfaceVisualization(std::move(pinocchioInterfaceInternal_), geometryInterface, nodeHandle)); } - std::cout << "[MobileManipulatorVisualization::launchVisualizerNode] jointStateMsgName_: " << jointStateMsgName_ << std::endl; - std::cout << "[MobileManipulatorVisualization::launchVisualizerNode] Waiting for initTFTransformFlag_ and initJointStateFlag_..." << std::endl; + //std::cout << "[MobileManipulatorVisualization::launchVisualizerNode] jointStateMsgName_: " << jointStateMsgName_ << std::endl; + //std::cout << "[MobileManipulatorVisualization::launchVisualizerNode] Waiting for initTFTransformFlag_ and initJointStateFlag_..." << std::endl; while(!initTFTransformFlag_ || !initJointStateFlag_){ros::spinOnce();} - std::cout << "[MobileManipulatorVisualization::launchVisualizerNode] END" << std::endl; + //std::cout << "[MobileManipulatorVisualization::launchVisualizerNode] END" << std::endl; } //------------------------------------------------------------------------------------------------------- @@ -170,7 +170,7 @@ void MobileManipulatorVisualization::launchVisualizerNode(ros::NodeHandle& nodeH //------------------------------------------------------------------------------------------------------- void MobileManipulatorVisualization::tfCallback(const tf2_msgs::TFMessage::ConstPtr& msg) { - //std::cerr << "[MobileManipulatorVisualization::tfCallback] START " << std::endl; + //std::cout << "[MobileManipulatorVisualization::tfCallback] START " << std::endl; try { @@ -194,7 +194,7 @@ void MobileManipulatorVisualization::tfCallback(const tf2_msgs::TFMessage::Const ROS_ERROR("%s", ex.what()); } - //std::cerr << "[MobileManipulatorVisualization::tfCallback] END " << std::endl; + //std::cout << "[MobileManipulatorVisualization::tfCallback] END " << std::endl; } //------------------------------------------------------------------------------------------------------- @@ -474,7 +474,7 @@ void MobileManipulatorVisualization::publishSelfCollisionDistances(const ocs2::v //------------------------------------------------------------------------------------------------------- void MobileManipulatorVisualization::getPoint(std::string& baseFrameName, std::string& pointFrameName, geometry_msgs::Point p) { - //std::cerr << "[MobileManipulatorVisualization::getPoint] START " << std::endl; + //std::cout << "[MobileManipulatorVisualization::getPoint] START " << std::endl; tf::StampedTransform tf_point_wrt_base; try @@ -488,7 +488,7 @@ void MobileManipulatorVisualization::getPoint(std::string& baseFrameName, std::s ROS_ERROR("%s", ex.what()); } - //std::cerr << "[MobileManipulatorVisualization::getPoint] END " << std::endl; + //std::cout << "[MobileManipulatorVisualization::getPoint] END " << std::endl; } //------------------------------------------------------------------------------------------------------- @@ -529,11 +529,13 @@ void MobileManipulatorVisualization::updateStateIndexMap() } } + /* std::cout << "[MobileManipulatorVisualization::updateStateIndexMap] stateIndexMap_ size: " << stateIndexMap_.size() << std::endl; for (int i = 0; i < stateIndexMap_.size(); ++i) { std::cout << i << " -> " << stateIndexMap_[i] << std::endl; } + */ //std::cout << "[MobileManipulatorVisualization::updateStateIndexMap] DEBUG INF" << std::endl; //while(1); @@ -1055,7 +1057,7 @@ void MobileManipulatorVisualization::publishTargetTrajectories(const ros::Time& //------------------------------------------------------------------------------------------------------- void MobileManipulatorVisualization::publishOptimizedTrajectory(const ros::Time& timeStamp, const PrimalSolution& policy) { - std::cout << "[MobileManipulatorVisualization::publishOptimizedTrajectory(2)] START" << std::endl; + //std::cout << "[MobileManipulatorVisualization::publishOptimizedTrajectory(2)] START" << std::endl; std::cout << "[MobileManipulatorVisualization::publishOptimizedTrajectory(2)] DEBUG INF" << std::endl; while(1); @@ -1119,7 +1121,7 @@ void MobileManipulatorVisualization::publishOptimizedTrajectory(const ros::Time& stateOptimizedPublisher_.publish(markerArray); stateOptimizedPosePublisher_.publish(poseArray); - std::cout << "[MobileManipulatorVisualization::publishOptimizedTrajectory(2)] END" << std::endl; + //std::cout << "[MobileManipulatorVisualization::publishOptimizedTrajectory(2)] END" << std::endl; } //------------------------------------------------------------------------------------------------------- diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorNode.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorNode.cpp index cb0566e80..d0af7c316 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorNode.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorNode.cpp @@ -1,4 +1,4 @@ -// LAST UPDATE: 2023.08.24 +// LAST UPDATE: 2023.10.24 // // AUTHOR: Neset Unver Akmandor (NUA) // @@ -28,13 +28,12 @@ using namespace mobile_manipulator; int main(int argc, char** argv) { - std::cout << "[MobileManipulatorNode::main] START" << std::endl; + //std::cout << "[MobileManipulatorNode::main] START" << std::endl; - //std::string robotName = "jackal_ur5"; - std::string robotModelName = "mobile_manipulator"; + bool printOutFlag = false; // Initialize ros node - ros::init(argc, argv, robotModelName + "_mpc"); + ros::init(argc, argv, "mobile_manipulator_node"); ros::NodeHandle nh_interface; //ros::NodeHandle nh_mpc; //ros::NodeHandle nh_mrt; @@ -48,25 +47,28 @@ int main(int argc, char** argv) nh_interface.getParam("/libFolder", libFolder); nh_interface.getParam("/urdfFile", urdfFile); - std::cout << "[MobileManipulatorNode::main] Loading task file: " << taskFile << std::endl; - std::cout << "[MobileManipulatorNode::main] Loading library folder: " << libFolder << std::endl; - std::cout << "[MobileManipulatorNode::main] Loading urdf file: " << urdfFile << std::endl; + if (printOutFlag) + { + std::cout << "[MobileManipulatorNode::main] Loading task file: " << taskFile << std::endl; + std::cout << "[MobileManipulatorNode::main] Loading library folder: " << libFolder << std::endl; + std::cout << "[MobileManipulatorNode::main] Loading urdf file: " << urdfFile << std::endl; + } // Robot interface int initModelModeInt = 2; - std::cout << "[MobileManipulatorNode::main] START interface" << std::endl; + //std::cout << "[MobileManipulatorNode::main] START interface" << std::endl; MobileManipulatorInterface m4_interface(nh_interface, taskFile, libFolder, urdfFile, initModelModeInt); double mpc_dt = 0.01; double mrt_dt = 0.01; - std::cout << "[MobileManipulatorNode::main] BEFORE mpcTimer " << std::endl; + //std::cout << "[MobileManipulatorNode::main] BEFORE mpcTimer " << std::endl; ros::Timer mpcTimer = nh_interface.createTimer(ros::Duration(mpc_dt), &MobileManipulatorInterface::mpcCallback, &m4_interface); - std::cout << "[MobileManipulatorNode::main] AFTER mpcTimer" << std::endl; + //std::cout << "[MobileManipulatorNode::main] AFTER mpcTimer" << std::endl; ros::Timer mrtTimer = nh_interface.createTimer(ros::Duration(mrt_dt), &MobileManipulatorInterface::mrtCallback, &m4_interface); - std::cout << "[MobileManipulatorNode::main] END" << std::endl; + //std::cout << "[MobileManipulatorNode::main] END" << std::endl; spinner.spin(); // spin() will not return until the node has been shutdown //ros::spin(); diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorTarget.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorTarget.cpp index 7f57a444e..a59652c9d 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorTarget.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorTarget.cpp @@ -36,10 +36,9 @@ TargetTrajectories goalPoseToTargetTrajectories(const Eigen::Vector3d& position, int main(int argc, char* argv[]) { - cout << "[MobileManipulatorTarget::main] START" << endl; + //cout << "[MobileManipulatorTarget::main] START" << endl; - const std::string robotMode = "mobile_manipulator"; - ros::init(argc, argv, robotMode + "_target"); + ros::init(argc, argv, "mobile_manipulator_target"); ros::MultiThreadedSpinner spinner(4); @@ -53,11 +52,19 @@ int main(int argc, char* argv[]) ros::NodeHandle pnh("~"); // INITIALIZE AND SET PARAMETERS - std::string world_frame_name, gz_model_msg_name, robot_name, drop_target_name; + std::string ns, topicPrefix, taskFile, world_frame_name, gz_model_msg_name, robot_name, drop_target_name; std::vector name_pkgs_ign, name_pkgs_man, scan_data_path_pkgs_ign, scan_data_path_pkgs_man, target_names; double map_resolution; bool drlFlag, printOutFlag = false; + ns = nh.getNamespace(); + topicPrefix = "mobile_manipulator_"; + ns += "/"; + if (ns != "") + { + topicPrefix = ns; + } + //pnh.param("/world_frame_name", world_frame_name, ""); //pnh.param("/gz_model_msg_name", gz_model_msg_name, ""); robot_name = "mobiman"; @@ -68,7 +75,6 @@ int main(int argc, char* argv[]) //cout << "[MobileManipulatorTarget::main] DEBUG INF" << endl; //while(1); - std::string taskFile; nh.getParam("/taskFile", taskFile); // read the task file @@ -78,21 +84,21 @@ int main(int argc, char* argv[]) loadData::loadPtreeValue(pt, world_frame_name, "model_information.worldFrame", printOutFlag); loadData::loadPtreeValue(pt, drlFlag, "model_settings.drlFlag", printOutFlag); - cout << "[MobileManipulatorTarget::main] world_frame_name: " << world_frame_name << endl; - cout << "[MobileManipulatorTarget::main] drlFlag: " << drlFlag << endl; - cout << "[MobileManipulatorTarget::main] gz_model_msg_name: " << gz_model_msg_name << endl; + //cout << "[MobileManipulatorTarget::main] world_frame_name: " << world_frame_name << endl; + //cout << "[MobileManipulatorTarget::main] drlFlag: " << drlFlag << endl; + //cout << "[MobileManipulatorTarget::main] gz_model_msg_name: " << gz_model_msg_name << endl; // Set TargetTrajectoriesGazebo - TargetTrajectoriesGazebo gu(nh, robotMode, gz_model_msg_name, robot_name, target_names, drop_target_name, &goalPoseToTargetTrajectories); + TargetTrajectoriesGazebo gu(nh, ns, topicPrefix, gz_model_msg_name, robot_name, target_names, drop_target_name, &goalPoseToTargetTrajectories); if (drlFlag) { - cout << "[MobileManipulatorTarget::main] DRL MODE IS ON!" << endl; + //cout << "[MobileManipulatorTarget::main] DRL MODE IS ON!" << endl; gu.setTaskMode(1); } else { - cout << "[MobileManipulatorTarget::main] MANUAL MODE IS ON!" << endl; + //cout << "[MobileManipulatorTarget::main] MANUAL MODE IS ON!" << endl; gu.initializeInteractiveMarkerTarget(); gu.initializeInteractiveMarkerAutoTarget(); gu.initializeInteractiveMarkerModelMode(); @@ -127,7 +133,7 @@ int main(int argc, char* argv[]) //TargetTrajectoriesInteractiveMarker targetPoseCommand(nh, robotMode, &goalPoseToTargetTrajectories); //targetPoseCommand.publishInteractiveMarker(); - cout << "[MobileManipulatorTarget::main] END" << endl; + //cout << "[MobileManipulatorTarget::main] END" << endl; // Successful exit return 0; diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesGazebo.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesGazebo.h index dcaa909a2..1d94caf35 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesGazebo.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesGazebo.h @@ -50,6 +50,7 @@ class TargetTrajectoriesGazebo final * @param [in] goalPoseToTargetTrajectories: A function which transforms the commanded pose to TargetTrajectories. */ TargetTrajectoriesGazebo(ros::NodeHandle& nodeHandle, + const std::string& ns, const std::string& topicPrefix, const std::string& gazeboModelMsgName, std::string robotName, diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Gazebo_Loop.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Gazebo_Loop.h index c5c4bebe9..870ce90e5 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Gazebo_Loop.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Gazebo_Loop.h @@ -87,7 +87,7 @@ class MRT_ROS_Gazebo_Loop */ ~MRT_ROS_Gazebo_Loop() { - std::cout << "[MRT_ROS_Gazebo_Loop::~MRT_ROS_Gazebo_Loop] SHUTTING DOWN..." << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::~MRT_ROS_Gazebo_Loop] SHUTTING DOWN..." << std::endl; } /** NUA TODO: Add description */ diff --git a/ocs2_ros_interfaces/src/command/TargetTrajectoriesGazebo.cpp b/ocs2_ros_interfaces/src/command/TargetTrajectoriesGazebo.cpp index b0504db08..5b5c5d471 100644 --- a/ocs2_ros_interfaces/src/command/TargetTrajectoriesGazebo.cpp +++ b/ocs2_ros_interfaces/src/command/TargetTrajectoriesGazebo.cpp @@ -17,6 +17,7 @@ namespace ocs2 { //------------------------------------------------------------------------------------------------------- //------------------------------------------------------------------------------------------------------- TargetTrajectoriesGazebo::TargetTrajectoriesGazebo(ros::NodeHandle& nodeHandle, + const std::string& ns, const std::string& topicPrefix, const std::string& gazeboModelMsgName, std::string robotName, @@ -49,11 +50,13 @@ TargetTrajectoriesGazebo::TargetTrajectoriesGazebo(ros::NodeHandle& nodeHandle, 0.0, 1.0, 0.0; //dropOrientationOffsetMatrix_ = graspOrientationOffsetMatrix_; + /* std::cout << "[TargetTrajectoriesGazebo::statusModelModeMPCCallback] targetNames_: " << std::endl; for (size_t i = 0; i < targetNames_.size(); i++) { std::cout << i << " -> " << targetNames_[i] << std::endl; } + */ //std::cout << "[TargetTrajectoriesGazebo::statusModelModeMPCCallback] DEBUG INF" << std::endl; //while(1); @@ -72,7 +75,7 @@ TargetTrajectoriesGazebo::TargetTrajectoriesGazebo(ros::NodeHandle& nodeHandle, latestObservation_ = ros_msg_conversions::readObservationMsg(*msg); policyReceivedFlag_ = true; }; - observationSubscriber_ = nodeHandle.subscribe(topicPrefix + "_mpc_observation", 1, observationCallback); + observationSubscriber_ = nodeHandle.subscribe(topicPrefix + "mpc_observation", 1, observationCallback); auto statusModelModeMPCCallback = [this](const std_msgs::Bool::ConstPtr& msg) { @@ -86,7 +89,7 @@ TargetTrajectoriesGazebo::TargetTrajectoriesGazebo(ros::NodeHandle& nodeHandle, //std::cout << "[TargetTrajectoriesGazebo::statusModelModeMPCCallback] END" << std::endl; //std::cout << "" << std::endl; }; - statusModelModeMPCSubscriber_ = nodeHandle.subscribe(topicPrefix + "_model_mode_mpc_status", 5, statusModelModeMPCCallback); + statusModelModeMPCSubscriber_ = nodeHandle.subscribe(topicPrefix + "model_mode_mpc_status", 5, statusModelModeMPCCallback); auto statusModelModeMRTCallback = [this](const std_msgs::Bool::ConstPtr& msg) { @@ -100,26 +103,30 @@ TargetTrajectoriesGazebo::TargetTrajectoriesGazebo(ros::NodeHandle& nodeHandle, //std::cout << "[TargetTrajectoriesGazebo::statusModelModeMRTCallback] END" << std::endl; //std::cout << "" << std::endl; }; - statusModelModeMRTSubscriber_ = nodeHandle.subscribe(topicPrefix + "_model_mode_mrt_status", 5, statusModelModeMRTCallback); + statusModelModeMRTSubscriber_ = nodeHandle.subscribe(topicPrefix + "model_mode_mrt_status", 5, statusModelModeMRTCallback); - gazeboModelStatesSubscriber_ = nodeHandle.subscribe(gazeboModelMsgName, 5, &TargetTrajectoriesGazebo::gazeboModelStatesCallback, this); + //std::cout << "[TargetTrajectoriesGazebo::statusModelModeMRTCallback] gazeboModelMsgName: " << gazeboModelMsgName << std::endl; + if (gazeboModelMsgName != "") + { + gazeboModelStatesSubscriber_ = nodeHandle.subscribe(gazeboModelMsgName, 5, &TargetTrajectoriesGazebo::gazeboModelStatesCallback, this); + } tfSubscriber_ = nodeHandle.subscribe("/tf", 5, &TargetTrajectoriesGazebo::tfCallback, this); /// Publishers targetTrajectoriesPublisherPtr_.reset(new TargetTrajectoriesRosPublisher(nodeHandle, topicPrefix)); - modelModePublisher_ = nodeHandle.advertise(topicPrefix + "_model_mode", 1, false); - goalMarkerArrayPublisher_ = nodeHandle.advertise(topicPrefix + "_goal", 10); - targetMarkerArrayPublisher_ = nodeHandle.advertise(topicPrefix + "_target", 10); + modelModePublisher_ = nodeHandle.advertise(ns + "model_mode", 1, false); + goalMarkerArrayPublisher_ = nodeHandle.advertise(ns + "goal", 10); + targetMarkerArrayPublisher_ = nodeHandle.advertise(ns + "target", 10); /// Clients //setTaskModeClient_ = nodeHandle.serviceClient("/set_task_mode"); - setTaskClient_ = nodeHandle.serviceClient("/set_task"); + setTaskClient_ = nodeHandle.serviceClient(ns + "set_task"); /// Services //setTaskModeService_ = nodeHandle.advertiseService("set_task_mode", &TargetTrajectoriesGazebo::setTaskModeSrv, this); - setPickedFlagService_ = nodeHandle.advertiseService("set_picked_flag", &TargetTrajectoriesGazebo::setPickedFlagSrv, this); - setSystemObservationService_ = nodeHandle.advertiseService("set_system_observation", &TargetTrajectoriesGazebo::setSystemObservationSrv, this); - setTargetDRLService_ = nodeHandle.advertiseService("set_target_drl", &TargetTrajectoriesGazebo::setTargetDRLSrv, this); + setPickedFlagService_ = nodeHandle.advertiseService(ns + "set_picked_flag", &TargetTrajectoriesGazebo::setPickedFlagSrv, this); + setSystemObservationService_ = nodeHandle.advertiseService(ns + "set_system_observation", &TargetTrajectoriesGazebo::setSystemObservationSrv, this); + setTargetDRLService_ = nodeHandle.advertiseService(ns + "set_target_drl", &TargetTrajectoriesGazebo::setTargetDRLSrv, this); while(!initTFCallbackFlag_){ros::spinOnce();} } @@ -346,7 +353,7 @@ void TargetTrajectoriesGazebo::initializeInteractiveMarkerDropTarget() //------------------------------------------------------------------------------------------------------- void TargetTrajectoriesGazebo::initializeInteractiveMarkerModelMode() { - std::cout << "[TargetTrajectoriesGazebo::initializeInteractiveMarkerModelMode] START" << std::endl; + //std::cout << "[TargetTrajectoriesGazebo::initializeInteractiveMarkerModelMode] START" << std::endl; //while (!initCallbackFlag_); @@ -363,7 +370,7 @@ void TargetTrajectoriesGazebo::initializeInteractiveMarkerModelMode() // 'commit' changes and send to all clients modelModeServer_.applyChanges(); - std::cout << "[TargetTrajectoriesGazebo::initializeInteractiveMarkerModelMode] END" << std::endl; + //std::cout << "[TargetTrajectoriesGazebo::initializeInteractiveMarkerModelMode] END" << std::endl; } //------------------------------------------------------------------------------------------------------- @@ -796,7 +803,7 @@ void TargetTrajectoriesGazebo::updateGoal(const Eigen::Vector3d& goalPos, const //------------------------------------------------------------------------------------------------------- void TargetTrajectoriesGazebo::updateTarget(bool autoFlag) { - std::cout << "[TargetTrajectoriesGazebo::updateTarget] START" << std::endl; + //std::cout << "[TargetTrajectoriesGazebo::updateTarget] START" << std::endl; Eigen::Vector3d targetPos; Eigen::Quaterniond targetOri; @@ -921,7 +928,7 @@ void TargetTrajectoriesGazebo::updateTarget(bool autoFlag) targetReadyFlag_ = true; - std::cout << "[TargetTrajectoriesGazebo::updateTarget] END" << std::endl << std::endl; + //std::cout << "[TargetTrajectoriesGazebo::updateTarget] END" << std::endl << std::endl; } //------------------------------------------------------------------------------------------------------- @@ -1073,6 +1080,7 @@ void TargetTrajectoriesGazebo::setTargetToEEPose() currentTargetOrientation_.z() = tf_ee_wrt_world.getRotation().z(); currentTargetOrientation_.w() = tf_ee_wrt_world.getRotation().w(); + /* std::cout << "[TargetTrajectoriesGazebo::updateEEPose] pos x: " << currentTargetPosition_.x() << std::endl; std::cout << "[TargetTrajectoriesGazebo::updateEEPose] pos y: " << currentTargetPosition_.y() << std::endl; std::cout << "[TargetTrajectoriesGazebo::updateEEPose] pos z: " << currentTargetPosition_.z() << std::endl; @@ -1081,6 +1089,7 @@ void TargetTrajectoriesGazebo::setTargetToEEPose() std::cout << "[TargetTrajectoriesGazebo::updateEEPose] ori y: " << currentTargetOrientation_.y() << std::endl; std::cout << "[TargetTrajectoriesGazebo::updateEEPose] ori z: " << currentTargetOrientation_.z() << std::endl; std::cout << "[TargetTrajectoriesGazebo::updateEEPose] ori w: " << currentTargetOrientation_.w() << std::endl; + */ //std::cout << "[TargetTrajectoriesGazebo::updateEEPose] END" << std::endl; } @@ -1797,7 +1806,7 @@ void TargetTrajectoriesGazebo::createMenuModelMode() //------------------------------------------------------------------------------------------------------- void TargetTrajectoriesGazebo::processFeedbackTarget(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) { - std::cout << "[TargetTrajectoriesGazebo::processFeedbackTarget] START" << std::endl; + //std::cout << "[TargetTrajectoriesGazebo::processFeedbackTarget] START" << std::endl; // Set task mode taskMode_ = 0; @@ -1845,7 +1854,7 @@ void TargetTrajectoriesGazebo::processFeedbackTarget(const visualization_msgs::I target.orientation.z = orientation.z(); target.orientation.w = orientation.w(); bool taskModeSuccess = setTask(taskMode_, target); - std::cout << "[TargetTrajectoriesGazebo::processFeedbackTarget] taskModeSuccess: " << taskModeSuccess << std::endl; + //std::cout << "[TargetTrajectoriesGazebo::processFeedbackTarget] taskModeSuccess: " << taskModeSuccess << std::endl; //targetReadyFlag_ = true; @@ -1859,7 +1868,7 @@ void TargetTrajectoriesGazebo::processFeedbackTarget(const visualization_msgs::I // Publish target trajectories //targetTrajectoriesPublisherPtr_->publishTargetTrajectories(targetTrajectories2); - std::cout << "[TargetTrajectoriesGazebo::processFeedbackTarget] END" << std::endl << std::endl; + //std::cout << "[TargetTrajectoriesGazebo::processFeedbackTarget] END" << std::endl << std::endl; } //------------------------------------------------------------------------------------------------------- @@ -1882,7 +1891,7 @@ void TargetTrajectoriesGazebo::processFeedbackAutoTarget(const visualization_msg // Update target updateGoal(true); - std::cout << "[TargetTrajectoriesGazebo::processFeedbackAutoTarget] taskMode_: " << taskMode_ << std::endl; + //std::cout << "[TargetTrajectoriesGazebo::processFeedbackAutoTarget] taskMode_: " << taskMode_ << std::endl; // Set desired state trajectory Eigen::Vector3d position; @@ -1926,7 +1935,7 @@ void TargetTrajectoriesGazebo::processFeedbackAutoTarget(const visualization_msg target.orientation.z = orientation.z(); target.orientation.w = orientation.w(); bool taskModeSuccess = setTask(taskMode_, target); - std::cout << "[TargetTrajectoriesGazebo::processFeedbackTarget] taskModeSuccess: " << taskModeSuccess << std::endl; + //std::cout << "[TargetTrajectoriesGazebo::processFeedbackTarget] taskModeSuccess: " << taskModeSuccess << std::endl; //targetReadyFlag_ = true; @@ -2017,7 +2026,7 @@ void TargetTrajectoriesGazebo::processFeedbackModelMode(const visualization_msgs bool TargetTrajectoriesGazebo::setPickedFlagSrv(ocs2_msgs::setBool::Request &req, ocs2_msgs::setBool::Response &res) { - std::cout << "[TargetTrajectoriesGazebo::setPickedFlagSrv] START" << std::endl; + //std::cout << "[TargetTrajectoriesGazebo::setPickedFlagSrv] START" << std::endl; pickedFlag_ = req.val; res.success = true; @@ -2033,7 +2042,7 @@ bool TargetTrajectoriesGazebo::setPickedFlagSrv(ocs2_msgs::setBool::Request &req updateGoal(true); std::cout << "[TargetTrajectoriesGazebo::setPickedFlagSrv] pickedFlag_: " << pickedFlag_ << std::endl; - std::cout << "[TargetTrajectoriesGazebo::setPickedFlagSrv] END" << std::endl; + //std::cout << "[TargetTrajectoriesGazebo::setPickedFlagSrv] END" << std::endl; return res.success; } @@ -2043,13 +2052,13 @@ bool TargetTrajectoriesGazebo::setPickedFlagSrv(ocs2_msgs::setBool::Request &req bool TargetTrajectoriesGazebo::setSystemObservationSrv(ocs2_msgs::setSystemObservation::Request &req, ocs2_msgs::setSystemObservation::Response &res) { - std::cout << "[TargetTrajectoriesGazebo::setSystemObservationSrv] START" << std::endl; + //std::cout << "[TargetTrajectoriesGazebo::setSystemObservationSrv] START" << std::endl; latestObservation_ = ros_msg_conversions::readObservationMsg(req.obs); res.success = true; policyReceivedFlag_ = true; - std::cout << "[TargetTrajectoriesGazebo::setSystemObservationSrv] END" << std::endl; + //std::cout << "[TargetTrajectoriesGazebo::setSystemObservationSrv] END" << std::endl; return res.success; } @@ -2059,7 +2068,7 @@ bool TargetTrajectoriesGazebo::setSystemObservationSrv(ocs2_msgs::setSystemObser bool TargetTrajectoriesGazebo::setTargetDRLSrv(ocs2_msgs::setTask::Request &req, ocs2_msgs::setTask::Response &res) { - std::cout << "[TargetTrajectoriesGazebo::setTargetDRLSrv] START" << std::endl; + //std::cout << "[TargetTrajectoriesGazebo::setTargetDRLSrv] START" << std::endl; geometry_msgs::Pose targetPose = req.targetPose; res.success = true; @@ -2080,7 +2089,7 @@ bool TargetTrajectoriesGazebo::setTargetDRLSrv(ocs2_msgs::setTask::Request &req, target.orientation.w = targetPose.orientation.w; //bool taskModeSuccess = setTask(taskMode_, target); - std::cout << "[TargetTrajectoriesGazebo::setTargetDRLSrv] END" << std::endl; + //std::cout << "[TargetTrajectoriesGazebo::setTargetDRLSrv] END" << std::endl; return res.success; } @@ -2089,7 +2098,7 @@ bool TargetTrajectoriesGazebo::setTargetDRLSrv(ocs2_msgs::setTask::Request &req, //------------------------------------------------------------------------------------------------------- bool TargetTrajectoriesGazebo::setTask(int taskMode, geometry_msgs::Pose targetPose) { - std::cout << "[TargetTrajectoriesGazebo::setTaskMode] START" << std::endl; + //std::cout << "[TargetTrajectoriesGazebo::setTask] START" << std::endl; bool success = false; ocs2_msgs::setTask srv; @@ -2107,7 +2116,7 @@ bool TargetTrajectoriesGazebo::setTask(int taskMode, geometry_msgs::Pose targetP success = false; } - std::cout << "[TargetTrajectoriesGazebo::setTask] END" << std::endl; + //std::cout << "[TargetTrajectoriesGazebo::setTask] END" << std::endl; return success; } diff --git a/ocs2_ros_interfaces/src/command/TargetTrajectoriesRosPublisher.cpp b/ocs2_ros_interfaces/src/command/TargetTrajectoriesRosPublisher.cpp index 92ab2c1af..80d462d97 100644 --- a/ocs2_ros_interfaces/src/command/TargetTrajectoriesRosPublisher.cpp +++ b/ocs2_ros_interfaces/src/command/TargetTrajectoriesRosPublisher.cpp @@ -39,9 +39,9 @@ namespace ocs2 { /******************************************************************************************************/ TargetTrajectoriesRosPublisher::TargetTrajectoriesRosPublisher(::ros::NodeHandle& nodeHandle, const std::string& topicPrefix) { - targetTrajectoriesPublisher_ = nodeHandle.advertise(topicPrefix + "_mpc_target", 1, false); + targetTrajectoriesPublisher_ = nodeHandle.advertise(topicPrefix + "mpc_target", 1, false); ros::spinOnce(); - ROS_INFO_STREAM("[TargetTrajectoriesRosPublisher::TargetTrajectoriesRosPublisher] The TargetTrajectories is publishing on " + topicPrefix + "_mpc_target topic."); + ROS_INFO_STREAM("[TargetTrajectoriesRosPublisher::TargetTrajectoriesRosPublisher] The TargetTrajectories is publishing on " + topicPrefix + "mpc_target topic."); } /******************************************************************************************************/ diff --git a/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp b/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp index 5bb06b1a7..aebf283eb 100644 --- a/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp +++ b/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp @@ -91,7 +91,7 @@ MPC_ROS_Interface::MPC_ROS_Interface(MPC_BASE& mpc_baseMotion, //------------------------------------------------------------------------------------------------------- MPC_ROS_Interface::~MPC_ROS_Interface() { - std::cout << "[MPC_ROS_Interface::~MPC_ROS_Interface] SHUTTING DOWN..." << std::endl; + //std::cout << "[MPC_ROS_Interface::~MPC_ROS_Interface] SHUTTING DOWN..." << std::endl; shutdownNode(); } @@ -164,11 +164,13 @@ bool MPC_ROS_Interface::resetMpcCallback(ocs2_msgs::reset::Request& req, ocs2_ms resetMpcNode(std::move(targetTrajectories)); res.done = static_cast(true); - std::cerr << "\n#####################################################" + /* + std::cout << "\n#####################################################" << "\n#####################################################" << "\n################# MPC is reset. ###################" << "\n#####################################################" << "\n#####################################################\n"; + */ return true; } else @@ -358,7 +360,7 @@ void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::mpc_observation: if (!resetRequestedEver_.load()) { - ROS_WARN_STREAM("[MPC_ROS_Interface::mpcObservationCallback] MPC should be reset first. Either call MPC_ROS_Interface::reset() or use the reset service."); + //std::cout << "[MPC_ROS_Interface::mpcObservationCallback] MPC should be reset first. Either call MPC_ROS_Interface::reset() or use the reset service." << std::endl; return; } @@ -423,17 +425,17 @@ void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::mpc_observation: if (timeWindow < 2.0 * mpcTimer_.getAverageInMilliseconds() * 1e-3) { - std::cerr << "[MPC_ROS_Interface::mpcObservationCallback] WARNING: The solution time window might be shorter than the MPC delay!\n"; + std::cout << "[MPC_ROS_Interface::mpcObservationCallback] WARNING: The solution time window might be shorter than the MPC delay!\n"; } // Display time benchmarks if (mpc_.settings().debugPrint_) { - std::cerr << '\n'; - std::cerr << "\n### MPC_ROS Benchmarking"; - std::cerr << "\n### Maximum : " << mpcTimer_.getMaxIntervalInMilliseconds() << "[ms]."; - std::cerr << "\n### Average : " << mpcTimer_.getAverageInMilliseconds() << "[ms]."; - std::cerr << "\n### Latest : " << mpcTimer_.getLastIntervalInMilliseconds() << "[ms]." << std::endl; + std::cout << '\n'; + std::cout << "\n### MPC_ROS Benchmarking"; + std::cout << "\n### Maximum : " << mpcTimer_.getMaxIntervalInMilliseconds() << "[ms]."; + std::cout << "\n### Average : " << mpcTimer_.getAverageInMilliseconds() << "[ms]."; + std::cout << "\n### Latest : " << mpcTimer_.getLastIntervalInMilliseconds() << "[ms]." << std::endl; } #ifdef PUBLISH_THREAD @@ -450,6 +452,7 @@ void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::mpc_observation: { if (!callbackEndFlag_) { + /* if (internalShutDownFlag_) { std::cout << "[MPC_ROS_Interface::mpcObservationCallback] INTERNALLY SHUT DOWN!!!: " << internalShutDownFlag_ << std::endl; @@ -457,6 +460,7 @@ void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::mpc_observation: std::cout << "[MPC_ROS_Interface::mpcObservationCallback] shutDownFlag_: " << shutDownFlag_ << std::endl; std::cout << "[MPC_ROS_Interface::mpcObservationCallback] internalShutDownFlag_: " << internalShutDownFlag_ << std::endl; std::cout << "[MPC_ROS_Interface::mpcObservationCallback] Shutting down..." << std::endl; + */ } callbackEndFlag_ = true; } @@ -468,10 +472,10 @@ void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::mpc_observation: //------------------------------------------------------------------------------------------------------- void MPC_ROS_Interface::shutdownNode() { - std::cout << "[MPC_ROS_Interface::shutdownNode] START" << std::endl; + //std::cout << "[MPC_ROS_Interface::shutdownNode] START" << std::endl; #ifdef PUBLISH_THREAD - ROS_INFO_STREAM("[MPC_ROS_Interface::shutdownNode] Shutting down workers ..."); + //std::cout << "[MPC_ROS_Interface::shutdownNode] Shutting down workers ..." << std::endl; std::unique_lock lk(publisherMutex_); terminateThread_ = true; @@ -483,16 +487,16 @@ void MPC_ROS_Interface::shutdownNode() publisherWorker_.join(); } - ROS_INFO_STREAM("[MPC_ROS_Interface::shutdownNode] All workers are shut down."); + //std::cout << "[MPC_ROS_Interface::shutdownNode] All workers are shut down." << std::endl; #endif // shutdown publishers mpcPolicyPublisher_.shutdown(); - std::cout << "[MPC_ROS_Interface::shutdownNode] BEFORE mpcObservationSubscriber_" << std::endl; + //std::cout << "[MPC_ROS_Interface::shutdownNode] BEFORE mpcObservationSubscriber_" << std::endl; mpcObservationSubscriber_.shutdown(); - std::cout << "[MPC_ROS_Interface::shutdownNode] END" << std::endl; + //std::cout << "[MPC_ROS_Interface::shutdownNode] END" << std::endl; } //------------------------------------------------------------------------------------------------------- @@ -500,7 +504,7 @@ void MPC_ROS_Interface::shutdownNode() //------------------------------------------------------------------------------------------------------- void MPC_ROS_Interface::spin() { - ROS_INFO_STREAM("[MPC_ROS_Interface::spin] Start spinning now..."); + //std::cout << "[MPC_ROS_Interface::spin] Start spinning now..." << std::endl; // Equivalent to ros::spin() + check if master is alive while (ros::ok() && ros::master::check() && !shutDownFlag_) { @@ -509,7 +513,7 @@ void MPC_ROS_Interface::spin() if (mpcShutDownFlag_ == "true") { - std::cout << "[MPC_ROS_Interface::spin] Shutting down..." << std::endl; + //std::cout << "[MPC_ROS_Interface::spin] Shutting down..." << std::endl; shutDownFlag_ = true; } @@ -524,10 +528,10 @@ void MPC_ROS_Interface::spin() //------------------------------------------------------------------------------------------------------- void MPC_ROS_Interface::launchNodes(ros::NodeHandle& nodeHandle) { - ROS_INFO_STREAM("[MPC_ROS_Interface::launchNodes] MPC node is setting up..."); + //std::cout << "[MPC_ROS_Interface::launchNodes] MPC node is setting up..." << std::endl; // Subscribe Observation - mpcObservationSubscriber_ = nodeHandle.subscribe(topicPrefix_ + "_mpc_observation", + mpcObservationSubscriber_ = nodeHandle.subscribe(topicPrefix_ + "mpc_observation", 1, &MPC_ROS_Interface::mpcObservationCallback, this, @@ -553,20 +557,21 @@ void MPC_ROS_Interface::launchNodes(ros::NodeHandle& nodeHandle) */ // Publish MPC Policy - mpcPolicyPublisher_ = nodeHandle.advertise(topicPrefix_ + "_mpc_policy", 1, true); + mpcPolicyPublisher_ = nodeHandle.advertise(topicPrefix_ + "mpc_policy", 1, true); // Publish Model Mode MPC Status - statusModelModeMPCPublisher_ = nodeHandle.advertise(topicPrefix_ + "_model_mode_mpc_status", 1, true); + statusModelModeMPCPublisher_ = nodeHandle.advertise(topicPrefix_ + "model_mode_mpc_status", 1, true); // Service Server to reset MPC - mpcResetServiceServer_ = nodeHandle.advertiseService(topicPrefix_ + "_mpc_reset", &MPC_ROS_Interface::resetMpcCallback, this); + mpcResetServiceServer_ = nodeHandle.advertiseService(topicPrefix_ + "mpc_reset", &MPC_ROS_Interface::resetMpcCallback, this); + /* #ifdef PUBLISH_THREAD - ROS_INFO_STREAM("[MPC_ROS_Interface::launchNodes] Publishing SLQ-MPC messages on a separate thread."); + std::cout << "[MPC_ROS_Interface::launchNodes] Publishing SLQ-MPC messages on a separate thread." << std::endl; #endif - - ROS_INFO_STREAM("[MPC_ROS_Interface::launchNodes] MPC node is ready."); - + std::cout << "[MPC_ROS_Interface::launchNodes] MPC node is ready." << std::endl; + */ + statusModelModeMPCMsg_.data = true; statusModelModeMPCPublisher_.publish(statusModelModeMPCMsg_); @@ -636,17 +641,17 @@ void MPC_ROS_Interface::run() if (timeWindow < 2.0 * mpcTimer_.getAverageInMilliseconds() * 1e-3) { - std::cerr << "[MPC_ROS_Interface::run] WARNING: The solution time window might be shorter than the MPC delay!\n"; + std::cout << "[MPC_ROS_Interface::run] WARNING: The solution time window might be shorter than the MPC delay!\n"; } // Display time benchmarks if (mpc_.settings().debugPrint_) { - std::cerr << '\n'; - std::cerr << "\n### MPC_ROS Benchmarking"; - std::cerr << "\n### Maximum : " << mpcTimer_.getMaxIntervalInMilliseconds() << "[ms]."; - std::cerr << "\n### Average : " << mpcTimer_.getAverageInMilliseconds() << "[ms]."; - std::cerr << "\n### Latest : " << mpcTimer_.getLastIntervalInMilliseconds() << "[ms]." << std::endl; + std::cout << '\n'; + std::cout << "\n### MPC_ROS Benchmarking"; + std::cout << "\n### Maximum : " << mpcTimer_.getMaxIntervalInMilliseconds() << "[ms]."; + std::cout << "\n### Average : " << mpcTimer_.getAverageInMilliseconds() << "[ms]."; + std::cout << "\n### Latest : " << mpcTimer_.getLastIntervalInMilliseconds() << "[ms]." << std::endl; } #ifdef PUBLISH_THREAD std::unique_lock lk(publisherMutex_); diff --git a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Gazebo_Loop.cpp b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Gazebo_Loop.cpp index 6605e4b1f..0625264bc 100644 --- a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Gazebo_Loop.cpp +++ b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Gazebo_Loop.cpp @@ -52,7 +52,7 @@ MRT_ROS_Gazebo_Loop::MRT_ROS_Gazebo_Loop(ros::NodeHandle& nh, robotModelInfo_(mrt.getRobotModelInfo()), dataPathReL_(logSavePathRel) { - std::cout << "[MRT_ROS_Gazebo_Loop::MRT_ROS_Gazebo_Loop] START" << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::MRT_ROS_Gazebo_Loop] START" << std::endl; timer1_.startTimer(); @@ -92,7 +92,7 @@ MRT_ROS_Gazebo_Loop::MRT_ROS_Gazebo_Loop(ros::NodeHandle& nh, //odometrySub_ = nh.subscribe("/jackal_velocity_controller/odom", 5, &MRT_ROS_Gazebo_Loop::odometryCallback, this); if (baseStateMsg == "") { - std::cout << "[MRT_ROS_Gazebo_Loop::MRT_ROS_Gazebo_Loop] DEFAULT: Base state info is acquired by /tf msg." << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::MRT_ROS_Gazebo_Loop] DEFAULT: Base state info is acquired by /tf msg." << std::endl; tfFlag_ = true; tfSub_ = nh.subscribe("/tf", 5, &MRT_ROS_Gazebo_Loop::tfCallback, this); } @@ -101,14 +101,14 @@ MRT_ROS_Gazebo_Loop::MRT_ROS_Gazebo_Loop(ros::NodeHandle& nh, linkStateSub_ = nh.subscribe(baseStateMsg, 5, &MRT_ROS_Gazebo_Loop::linkStateCallback, this); } - std::cout << "[MRT_ROS_Gazebo_Loop::MRT_ROS_Gazebo_Loop] armStateMsg: " << armStateMsg << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::MRT_ROS_Gazebo_Loop] armStateMsg: " << armStateMsg << std::endl; jointStateSub_ = nh.subscribe(armStateMsg, 5, &MRT_ROS_Gazebo_Loop::jointStateCallback, this); //jointTrajectoryControllerStateSub_ = nh.subscribe(armStateMsg, 5, &MRT_ROS_Gazebo_Loop::jointTrajectoryControllerStateCallback, this); - std::cout << "[MRT_ROS_Gazebo_Loop::run] Waiting to subscribe selfCollisionInfoMsgName_: " << selfCollisionInfoMsgName_ << std::endl; - std::cout << "[MRT_ROS_Gazebo_Loop::run] Waiting to subscribe extCollisionInfoBaseMsgName_: " << extCollisionInfoBaseMsgName_ << std::endl; - std::cout << "[MRT_ROS_Gazebo_Loop::run] Waiting to subscribe extCollisionInfoArmMsgName_: " << extCollisionInfoArmMsgName_ << std::endl; - std::cout << "[MRT_ROS_Gazebo_Loop::run] Waiting to subscribe pointsOnRobotMsgName_: " << pointsOnRobotMsgName_ << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::run] Waiting to subscribe selfCollisionInfoMsgName_: " << selfCollisionInfoMsgName_ << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::run] Waiting to subscribe extCollisionInfoBaseMsgName_: " << extCollisionInfoBaseMsgName_ << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::run] Waiting to subscribe extCollisionInfoArmMsgName_: " << extCollisionInfoArmMsgName_ << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::run] Waiting to subscribe pointsOnRobotMsgName_: " << pointsOnRobotMsgName_ << std::endl; //std::cout << "[MRT_ROS_Gazebo_Loop::run] Waiting to subscribe goalMsgName_: "<< goalMsgName_ << std::endl; selfCollisionInfoSub_ = nh.subscribe(selfCollisionInfoMsgName_, 5, &MRT_ROS_Gazebo_Loop::selfCollisionInfoCallback, this); extCollisionInfoBaseSub_ = nh.subscribe(extCollisionInfoBaseMsgName_, 5, &MRT_ROS_Gazebo_Loop::extCollisionInfoBaseCallback, this); @@ -122,20 +122,20 @@ MRT_ROS_Gazebo_Loop::MRT_ROS_Gazebo_Loop(ros::NodeHandle& nh, currentTarget_.resize(7); // Publishers - baseTwistPub_ = nh.advertise(ns_ + baseControlMsg, 1); - armJointTrajectoryPub_ = nh.advertise(ns_ + armControlMsg, 1); + baseTwistPub_ = nh.advertise(baseControlMsg, 1); + armJointTrajectoryPub_ = nh.advertise(armControlMsg, 1); armJointVelocityPub_ = nh.advertise(ns_ + "/arm_controller/velocity", 1); /// Clients - attachClient_ = nh.serviceClient("/link_attacher_node/attach"); - detachClient_ = nh.serviceClient("/link_attacher_node/detach"); + attachClient_ = nh.serviceClient(ns_ + "/link_attacher_node/attach"); + detachClient_ = nh.serviceClient(ns_ + "/link_attacher_node/detach"); //setTaskModeClient_ = nh.serviceClient("/set_task_mode"); - setPickedFlagClient_ = nh.serviceClient("/set_picked_flag"); - setSystemObservationClient_ = nh.serviceClient("/set_system_observation"); + setPickedFlagClient_ = nh.serviceClient(ns_ + "/set_picked_flag"); + setSystemObservationClient_ = nh.serviceClient(ns_ + "/set_system_observation"); /// Services //setTaskModeService_ = nh.advertiseService("set_task_mode", &MRT_ROS_Gazebo_Loop::setTaskModeSrv, this); - setTaskService_ = nh.advertiseService("set_task", &MRT_ROS_Gazebo_Loop::setTaskSrv, this); + setTaskService_ = nh.advertiseService(ns_ + "/set_task", &MRT_ROS_Gazebo_Loop::setTaskSrv, this); if (mrtDesiredFrequency_ < 0) { @@ -147,7 +147,7 @@ MRT_ROS_Gazebo_Loop::MRT_ROS_Gazebo_Loop(ros::NodeHandle& nh, ROS_WARN_STREAM("[MRT_ROS_Gazebo_Loop::MRT_ROS_Gazebo_Loop] Warning: MPC loop is not realtime! For realtime setting, set mpcDesiredFrequency to any negative number."); } - std::cout << "[MRT_ROS_Gazebo_Loop::updateStateIndexMap] Waiting to subscribe armStateMsg..." << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::updateStateIndexMap] Waiting to subscribe armStateMsg..." << std::endl; //boost::shared_ptr jointStatePtrMsg = ros::topic::waitForMessage(armStateMsg); while(!initFlagArmState_){ros::spinOnce();} timer2_.startTimer(); @@ -171,7 +171,7 @@ MRT_ROS_Gazebo_Loop::MRT_ROS_Gazebo_Loop(ros::NodeHandle& nh, //std::cout << "[MRT_ROS_Gazebo_Loop::MRT_ROS_Gazebo_Loop] END" << std::endl; //while(1); - std::cout << "[MRT_ROS_Gazebo_Loop::MRT_ROS_Gazebo_Loop] END" << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::MRT_ROS_Gazebo_Loop] END" << std::endl; } //------------------------------------------------------------------------------------------------------- @@ -209,7 +209,7 @@ bool MRT_ROS_Gazebo_Loop::isStateInitialized() } default: - std::cerr << "[MRT_ROS_Gazebo_Loop::isStateInitialized] ERROR: Invalid robot model type!"; + std::cout << "[MRT_ROS_Gazebo_Loop::isStateInitialized] ERROR: Invalid robot model type!"; return false; } } @@ -219,28 +219,28 @@ bool MRT_ROS_Gazebo_Loop::isStateInitialized() //------------------------------------------------------------------------------------------------------- void MRT_ROS_Gazebo_Loop::run(vector_t initTarget) { - ROS_INFO_STREAM("[MRT_ROS_Gazebo_Loop::run] Waiting for the initial policy..."); + //std::cout << "[MRT_ROS_Gazebo_Loop::run] Waiting for the initial policy..." << std::endl; //currentTarget_ = initTarget; taskEndFlag_ = true; shutDownFlag_ = false; currentTarget_ = initTarget; - std::cout << "[MRT_ROS_Gazebo_Loop::run] Waiting for the state to be initialized..." << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::run] Waiting for the state to be initialized..." << std::endl; while(!isStateInitialized()){ros::spinOnce();} if (drlFlag_) { - std::cout << "[MRT_ROS_Gazebo_Loop::run] Waiting for the initFlagSelfCollision_, initFlagExtCollision_, initFlagPointsOnRobot_, initFlagGoal_ to be initialized..." << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::run] Waiting for the initFlagSelfCollision_, initFlagExtCollision_, initFlagPointsOnRobot_, initFlagGoal_ to be initialized..." << std::endl; while(!initFlagSelfCollision_ || !initFlagExtCollisionBase_ || !initFlagExtCollisionArm_ || !initFlagPointsOnRobot_){ros::spinOnce();} } - //ROS_INFO_STREAM("[MRT_ROS_Gazebo_Loop::run] BEFORE getCurrentObservation"); + //std::cout << ("[MRT_ROS_Gazebo_Loop::run] BEFORE getCurrentObservation") << std::endl; SystemObservation initObservation = getCurrentObservation(true); setSystemObservation(initObservation); - std::cout << "[MRT_ROS_Gazebo_Loop::run] Waiting for the target to be received..." << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::run] Waiting for the target to be received..." << std::endl; while(!targetReceivedFlag_ && !shutDownFlag_) { mrtShutDownFlag_ = getenv("mrtShutDownFlag"); @@ -253,7 +253,7 @@ void MRT_ROS_Gazebo_Loop::run(vector_t initTarget) vector_t currentTarget = currentTarget_; //currentTarget_ = initTarget; - std::cout << "[MRT_ROS_Gazebo_Loop::run] mrtShutDownFlag_: " << mrtShutDownFlag_ << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::run] mrtShutDownFlag_: " << mrtShutDownFlag_ << std::endl; if (!shutDownFlag_) { @@ -310,8 +310,8 @@ void MRT_ROS_Gazebo_Loop::run(vector_t initTarget) ros::spinOnce(); //std::cout << "[MRT_ROS_Gazebo_Loop::run] END INIT WHILE" << std::endl; } - ROS_INFO_STREAM("[MRT_ROS_Gazebo_Loop::run] Initial policy has been received."); - std::cout << "[MRT_ROS_Gazebo_Loop::run] modelMode: " << getModelModeString(robotModelInfo_) << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::run] Initial policy has been received." << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::run] modelMode: " << getModelModeString(robotModelInfo_) << std::endl; currentInput_ = initObservation.input; @@ -320,7 +320,7 @@ void MRT_ROS_Gazebo_Loop::run(vector_t initTarget) //std::cout << "[MRT_ROS_Gazebo_Loop::run] AFTER mrtLoop" << std::endl; } - std::cout << "[MRT_ROS_Gazebo_Loop::run] END" << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::run] END" << std::endl; } //------------------------------------------------------------------------------------------------------- @@ -355,6 +355,7 @@ void MRT_ROS_Gazebo_Loop::getInitTarget(vector_t& initTarget) tf_ee_wrt_world.getRotation().y(), tf_ee_wrt_world.getRotation().z()).coeffs(); + /* std::cout << "[MRT_ROS_Gazebo_Loop::getInitTarget] eeFrame: " << eeFrame << std::endl; std::cout << "[MRT_ROS_Gazebo_Loop::getInitTarget] initTarget.x: " << initTarget(0) << std::endl; std::cout << "[MRT_ROS_Gazebo_Loop::getInitTarget] initTarget.y: " << initTarget(1) << std::endl; @@ -363,6 +364,7 @@ void MRT_ROS_Gazebo_Loop::getInitTarget(vector_t& initTarget) std::cout << "[MRT_ROS_Gazebo_Loop::getInitTarget] initTarget.qy: " << initTarget(4) << std::endl; std::cout << "[MRT_ROS_Gazebo_Loop::getInitTarget] initTarget.qz: " << initTarget(5) << std::endl; std::cout << "[MRT_ROS_Gazebo_Loop::getInitTarget] initTarget.qw: " << initTarget(6) << std::endl; + */ } //------------------------------------------------------------------------------------------------------- @@ -415,7 +417,7 @@ void MRT_ROS_Gazebo_Loop::getCurrentState(vector_t& currentState) } default: - std::cerr << "[MRT_ROS_Gazebo_Loop::MRT_ROS_Gazebo_Loop] ERROR: Invalid robot model type!"; + std::cout << "[MRT_ROS_Gazebo_Loop::MRT_ROS_Gazebo_Loop] ERROR: Invalid robot model type!"; break; } } @@ -510,7 +512,7 @@ void MRT_ROS_Gazebo_Loop::mrtLoop() if (initPolicyCtr > initPolicyCtrMax_) { - std::cout << "[MRT_ROS_Gazebo_Loop::mrtLoop] THATS ENOUGH WAITING FOR THE POLICY!" << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::mrtLoop] THATS ENOUGH WAITING FOR THE POLICY!" << std::endl; shutDownFlag_ = true; } else @@ -587,7 +589,7 @@ void MRT_ROS_Gazebo_Loop::mrtLoop() //std::cout << "[MRT_ROS_Gazebo_Loop::mrtLoop] DEBUG INF" << std::endl; //while(1); - std::cout << "[MRT_ROS_Gazebo_Loop::mrtLoop] END" << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::mrtLoop] END" << std::endl; } //------------------------------------------------------------------------------------------------------- @@ -756,7 +758,7 @@ void MRT_ROS_Gazebo_Loop::updateStateIndexMap(bool updateStateIndexMapFlag) void MRT_ROS_Gazebo_Loop::tfCallback(const tf2_msgs::TFMessage::ConstPtr& msg) { - //std::cerr << "[MRT_ROS_Gazebo_Loop::tfCallback] START " << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::tfCallback] START " << std::endl; //tf2_msgs::TFMessage tf_msg = *msg; try @@ -781,7 +783,7 @@ void MRT_ROS_Gazebo_Loop::tfCallback(const tf2_msgs::TFMessage::ConstPtr& msg) initFlagBaseState_ = true; - //std::cerr << "[MRT_ROS_Gazebo_Loop::tfCallback] START " << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::tfCallback] START " << std::endl; } //------------------------------------------------------------------------------------------------------- @@ -982,7 +984,7 @@ void MRT_ROS_Gazebo_Loop::updateFullModelState() stateVelocityBase_.push_back(robotBaseTwistMsg_.linear.x); stateVelocityBase_.push_back(robotBaseTwistMsg_.angular.z); - //std::cerr << "[MRT_ROS_Gazebo_Loop::updateFullModelState] mrt_.getArmStateDim(): " << mrt_.getArmStateDim() << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::updateFullModelState] mrt_.getArmStateDim(): " << mrt_.getArmStateDim() << std::endl; // Set arm state //for (int i = 0; i < jointTrajectoryControllerStateMsg.joint_names.size(); ++i) @@ -1169,7 +1171,7 @@ SystemObservation MRT_ROS_Gazebo_Loop::getCurrentObservation(bool initFlag) } default: - std::cerr << "[MRT_ROS_Gazebo_Loop::getCurrentObservation] ERROR: Invalid model mode!"; + std::cout << "[MRT_ROS_Gazebo_Loop::getCurrentObservation] ERROR: Invalid model mode!"; while(1); break; } @@ -1178,7 +1180,7 @@ SystemObservation MRT_ROS_Gazebo_Loop::getCurrentObservation(bool initFlag) default: { - std::cerr << "[MRT_ROS_Gazebo_Loop::getCurrentObservation] ERROR: Invalid robot model type!"; + std::cout << "[MRT_ROS_Gazebo_Loop::getCurrentObservation] ERROR: Invalid robot model type!"; while(1); break; } @@ -1311,7 +1313,7 @@ bool MRT_ROS_Gazebo_Loop::setTaskMode(int val) //------------------------------------------------------------------------------------------------------- bool MRT_ROS_Gazebo_Loop::setPickedFlag(bool val) { - std::cout << "[MRT_ROS_Gazebo_Loop::setPickedFlag] START" << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::setPickedFlag] START" << std::endl; bool success = false; ocs2_msgs::setBool srv; @@ -1326,7 +1328,7 @@ bool MRT_ROS_Gazebo_Loop::setPickedFlag(bool val) success = false; } - std::cout << "[MRT_ROS_Gazebo_Loop::setPickedFlag] END" << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::setPickedFlag] END" << std::endl; return success; } @@ -1336,7 +1338,7 @@ bool MRT_ROS_Gazebo_Loop::setPickedFlag(bool val) //------------------------------------------------------------------------------------------------------- bool MRT_ROS_Gazebo_Loop::setSystemObservation(const SystemObservation& currentObservation) { - std::cout << "[MRT_ROS_Gazebo_Loop::setSystemObservation] START" << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::setSystemObservation] START" << std::endl; bool success = false; ocs2_msgs::setSystemObservation srv; @@ -1351,7 +1353,7 @@ bool MRT_ROS_Gazebo_Loop::setSystemObservation(const SystemObservation& currentO success = false; } - std::cout << "[MRT_ROS_Gazebo_Loop::setSystemObservation] END" << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::setSystemObservation] END" << std::endl; return success; } @@ -1379,7 +1381,7 @@ bool MRT_ROS_Gazebo_Loop::setTaskModeSrv(ocs2_msgs::setInt::Request &req, bool MRT_ROS_Gazebo_Loop::setTaskSrv(ocs2_msgs::setTask::Request &req, ocs2_msgs::setTask::Response &res) { - std::cout << "[MRT_ROS_Gazebo_Loop::setTaskModeSrv] START" << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::setTaskModeSrv] START" << std::endl; taskMode_ = req.taskMode; currentTargetName_ = req.targetName; currentTargetAttachLinkName_ = req.targetAttachLinkName; @@ -1395,8 +1397,8 @@ bool MRT_ROS_Gazebo_Loop::setTaskSrv(ocs2_msgs::setTask::Request &req, taskEndFlag_ = false; targetReceivedFlag_ = true; - std::cout << "[MRT_ROS_Gazebo_Loop::setTaskModeSrv] taskMode_: " << taskMode_ << std::endl; - std::cout << "[MRT_ROS_Gazebo_Loop::setTaskModeSrv] END" << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::setTaskModeSrv] taskMode_: " << taskMode_ << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::setTaskModeSrv] END" << std::endl; return res.success; } @@ -1557,7 +1559,7 @@ bool MRT_ROS_Gazebo_Loop::checkPickDrop() if (isPickDropPoseReached(taskMode)) { - std::cout << "[MRT_ROS_Gazebo_Loop::mrtLoop] START PICK/DROP" << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::mrtLoop] START PICK/DROP" << std::endl; gazebo_ros_link_attacher::Attach srv; srv.request.model_name_1 = robotModelInfo_.robotName; @@ -1570,12 +1572,12 @@ bool MRT_ROS_Gazebo_Loop::checkPickDrop() { if (attachClient_.call(srv)) { - std::cout << "[MRT_ROS_Gazebo_Loop::mrtLoop] response: " << srv.response.ok << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::mrtLoop] response: " << srv.response.ok << std::endl; taskMode = 2; pickedFlag_ = true; taskEndFlag_ = true; bool taskModeSuccess = setPickedFlag(pickedFlag_); - std::cout << "[MRT_ROS_Gazebo_Loop::mrtLoop] taskModeSuccess: " << taskModeSuccess << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::mrtLoop] taskModeSuccess: " << taskModeSuccess << std::endl; ros::spinOnce(); } else @@ -1585,15 +1587,15 @@ bool MRT_ROS_Gazebo_Loop::checkPickDrop() } else if (taskMode == 2) { - std::cout << "[MRT_ROS_Gazebo_Loop::mrtLoop] DROP" << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::mrtLoop] DROP" << std::endl; if (detachClient_.call(srv)) { - std::cout << "[MRT_ROS_Gazebo_Loop::mrtLoop] response: " << srv.response.ok << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::mrtLoop] response: " << srv.response.ok << std::endl; taskMode = 1; pickedFlag_ = false; taskEndFlag_ = true; bool taskModeSuccess = setPickedFlag(pickedFlag_); - std::cout << "[MRT_ROS_Gazebo_Loop::mrtLoop] taskModeSuccess: " << taskModeSuccess << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::mrtLoop] taskModeSuccess: " << taskModeSuccess << std::endl; ros::spinOnce(); } else @@ -1651,7 +1653,7 @@ bool MRT_ROS_Gazebo_Loop::checkCollision(bool enableShutDownFlag) { drlActionResult_ = 1; shutDownFlag_ = enableShutDownFlag; - std::cout << "[MRT_ROS_Gazebo_Loop::checkCollision] SELF COLLISION! dist: " << selfCollisionInfoMsg.distance[i] << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::checkCollision] SELF COLLISION! dist: " << selfCollisionInfoMsg.distance[i] << std::endl; return true; } } @@ -1679,7 +1681,7 @@ bool MRT_ROS_Gazebo_Loop::checkCollision(bool enableShutDownFlag) { drlActionResult_ = 1; shutDownFlag_ = enableShutDownFlag; - std::cout << "[MRT_ROS_Gazebo_Loop::checkCollision] EXT COLLISION BASE! dist: " << extCollisionInfoBaseMsg.distance[i] << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::checkCollision] EXT COLLISION BASE! dist: " << extCollisionInfoBaseMsg.distance[i] << std::endl; return true; } } @@ -1707,7 +1709,7 @@ bool MRT_ROS_Gazebo_Loop::checkCollision(bool enableShutDownFlag) { drlActionResult_ = 1; shutDownFlag_ = enableShutDownFlag; - std::cout << "[MRT_ROS_Gazebo_Loop::checkCollision] EXT COLLISION ARM! dist: " << extCollisionInfoArmMsg.distance[i] << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::checkCollision] EXT COLLISION ARM! dist: " << extCollisionInfoArmMsg.distance[i] << std::endl; return true; } } @@ -1720,7 +1722,7 @@ bool MRT_ROS_Gazebo_Loop::checkCollision(bool enableShutDownFlag) { drlActionResult_ = 1; shutDownFlag_ = enableShutDownFlag; - std::cout << "[MRT_ROS_Gazebo_Loop::checkCollision] GROUND COLLISION!" << pointsOnRobotMsg.markers[i].pose.position.z << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::checkCollision] GROUND COLLISION!" << pointsOnRobotMsg.markers[i].pose.position.z << std::endl; return true; } } @@ -1759,7 +1761,7 @@ bool MRT_ROS_Gazebo_Loop::checkRollover(bool enableShutDownFlag) { drlActionResult_ = 2; shutDownFlag_ = enableShutDownFlag; - std::cout << "[MRT_ROS_Gazebo_Loop::checkRollover] ROLL ROLLOVER!" << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::checkRollover] ROLL ROLLOVER!" << std::endl; return true; } @@ -1767,7 +1769,7 @@ bool MRT_ROS_Gazebo_Loop::checkRollover(bool enableShutDownFlag) { drlActionResult_ = 2; shutDownFlag_ = enableShutDownFlag; - std::cout << "[MRT_ROS_Gazebo_Loop::checkRollover] PITCH ROLLOVER!" << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::checkRollover] PITCH ROLLOVER!" << std::endl; return true; } @@ -1808,7 +1810,7 @@ bool MRT_ROS_Gazebo_Loop::checkGoal(bool enableShutDownFlag) drlActionResult_ = 3; shutDownFlag_ = enableShutDownFlag; - std::cout << "[MRT_ROS_Gazebo_Loop::checkGoal] GOALLLLLLLLLLLLLLLLLLLLL" << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::checkGoal] REACHED GOAL!" << std::endl; //std::cout << "[MRT_ROS_Gazebo_Loop::checkGoal] END true" << std::endl; return true; } @@ -1930,7 +1932,7 @@ bool MRT_ROS_Gazebo_Loop::checkTarget(bool enableShutDownFlag) if ((err_pos < err_threshold_pos_) && (err_ori < err_threshold_ori_yaw_)) { - std::cout << "[MRT_ROS_Gazebo_Loop::checkGoal] REACHED TO THE TARGET with modelModeInt: " << modelModeInt << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::checkGoal] REACHED TO THE TARGET with modelModeInt: " << modelModeInt << std::endl; drlActionResult_ = 4; shutDownFlag_ = enableShutDownFlag; //std::cout << "[MRT_ROS_Gazebo_Loop::checkTarget] END true" << std::endl; @@ -1948,7 +1950,7 @@ bool MRT_ROS_Gazebo_Loop::checkTarget(bool enableShutDownFlag) if ((err_pos < err_threshold_pos_) && (err_ori < err_threshold_ori_quat_)) { - std::cout << "[MRT_ROS_Gazebo_Loop::checkGoal] REACHED TO THE TARGET with modelModeInt: " << modelModeInt << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::checkGoal] REACHED TO THE TARGET with modelModeInt: " << modelModeInt << std::endl; drlActionResult_ = 4; shutDownFlag_ = enableShutDownFlag; //std::cout << "[MRT_ROS_Gazebo_Loop::checkTarget] END true" << std::endl; @@ -2002,29 +2004,29 @@ int MRT_ROS_Gazebo_Loop::checkTaskStatus(bool enableShutDownFlag) if (checkCollision(enableShutDownFlag)) { - std::cout << "[MRT_ROS_Gazebo_Loop::checkTaskStatus] checkTaskStatus: COLLISION!" << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::checkTaskStatus] checkTaskStatus: COLLISION!" << std::endl; return 1; } else if (checkRollover(enableShutDownFlag)) { - std::cout << "[MRT_ROS_Gazebo_Loop::checkTaskStatus] checkTaskStatus: ROLLOVER!" << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::checkTaskStatus] checkTaskStatus: ROLLOVER!" << std::endl; return 2; } else if (checkGoal(enableShutDownFlag)) { - std::cout << "[MRT_ROS_Gazebo_Loop::checkTaskStatus] checkTaskStatus: REACHED GOAL!" << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::checkTaskStatus] checkTaskStatus: REACHED GOAL!" << std::endl; return 3; } else if (checkTarget(enableShutDownFlag)) { - std::cout << "[MRT_ROS_Gazebo_Loop::checkTaskStatus] checkTaskStatus: REACHED TARGET!" << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::checkTaskStatus] checkTaskStatus: REACHED TARGET!" << std::endl; return 4; } else if (time_ > drlActionTimeHorizon_) { - std::cout << "[MRT_ROS_Gazebo_Loop::checkTaskStatus] time_: " << time_ << std::endl; - std::cout << "[MRT_ROS_Gazebo_Loop::checkTaskStatus] drlActionTimeHorizon_: " << drlActionTimeHorizon_ << std::endl; - std::cout << "[MRT_ROS_Gazebo_Loop::checkTaskStatus] END OF ACTION HORIZON!" << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::checkTaskStatus] time_: " << time_ << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::checkTaskStatus] drlActionTimeHorizon_: " << drlActionTimeHorizon_ << std::endl; + //std::cout << "[MRT_ROS_Gazebo_Loop::checkTaskStatus] END OF ACTION HORIZON!" << std::endl; shutDownFlag_ = enableShutDownFlag; drlActionResult_ = 5; diff --git a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp index 40cb4c655..7f431dbd7 100644 --- a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp +++ b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp @@ -53,7 +53,7 @@ MRT_ROS_Interface::MRT_ROS_Interface(RobotModelInfo& robotModelInfo, std::string /******************************************************************************************************/ MRT_ROS_Interface::~MRT_ROS_Interface() { - std::cout << "[MRT_ROS_Interface::~MRT_ROS_Interface] SHUTTING DOWN..." << std::endl; + //std::cout << "[MRT_ROS_Interface::~MRT_ROS_Interface] SHUTTING DOWN..." << std::endl; shutdownNodes(); } @@ -82,7 +82,7 @@ void MRT_ROS_Interface::resetMpcNode(const TargetTrajectories& initTargetTraject } mpcResetServiceClient_.call(resetSrv); - ROS_INFO_STREAM("[MRT_ROS_Interface::resetMpcNode] MPC node has been reset."); + //std::cout << "[MRT_ROS_Interface::resetMpcNode] MPC node has been reset." << std::endl; } /******************************************************************************************************/ @@ -268,11 +268,12 @@ void MRT_ROS_Interface::mpcPolicyCallback(const ocs2_msgs::mpc_flattened_control /******************************************************************************************************/ void MRT_ROS_Interface::shutdownNodes() { - std::cout << "[MRT_ROS_Interface::shutdownNodes] START" << std::endl; + //std::cout << "[MRT_ROS_Interface::shutdownNodes] START" << std::endl; + #ifdef PUBLISH_THREAD - ROS_INFO_STREAM("[MRT_ROS_Interface::shutdownNodes] Shutting down workers..."); + //std::cout << "[MRT_ROS_Interface::shutdownNodes] Shutting down workers..." << std::endl; shutdownPublisher(); - ROS_INFO_STREAM("[MRT_ROS_Interface::shutdownNodes] All workers are shut down."); + //std::cout << "[MRT_ROS_Interface::shutdownNodes] All workers are shut down." << std::endl; #endif // clean up callback queue @@ -284,7 +285,7 @@ void MRT_ROS_Interface::shutdownNodes() setenv("mrtExitFlag", "true", 1); - std::cout << "[MRT_ROS_Interface::shutdownNodes] END" << std::endl; + //std::cout << "[MRT_ROS_Interface::shutdownNodes] END" << std::endl; } /******************************************************************************************************/ @@ -321,17 +322,17 @@ void MRT_ROS_Interface::launchNodes(ros::NodeHandle& nodeHandle) { this->reset(); - ROS_INFO_STREAM("[MRT_ROS_Interface::launchNodes] MRT node is setting up ..."); + //std::cout << "[MRT_ROS_Interface::launchNodes] MRT node is setting up ..." << std::endl; // Publish Observation - mpcObservationPublisher_ = nodeHandle.advertise(topicPrefix_ + "_mpc_observation", 1); + mpcObservationPublisher_ = nodeHandle.advertise(topicPrefix_ + "mpc_observation", 1); // Publish Model Mode MPC Status - statusModelModeMRTPublisher_ = nodeHandle.advertise(topicPrefix_ + "_model_mode_mrt_status", 1, true); + statusModelModeMRTPublisher_ = nodeHandle.advertise(topicPrefix_ + "model_mode_mrt_status", 1, true); // Subscribe Policy auto ops = ros::SubscribeOptions::create( - topicPrefix_ + "_mpc_policy", // topic name + topicPrefix_ + "mpc_policy", // topic name 1, // queue length boost::bind(&MRT_ROS_Interface::mpcPolicyCallback, this, boost::placeholders::_1), // callback ros::VoidConstPtr(), // tracked object @@ -364,14 +365,15 @@ void MRT_ROS_Interface::launchNodes(ros::NodeHandle& nodeHandle) */ // Service client to reset MPC - mpcResetServiceClient_ = nodeHandle.serviceClient(topicPrefix_ + "_mpc_reset"); + mpcResetServiceClient_ = nodeHandle.serviceClient(topicPrefix_ + "mpc_reset"); + /* #ifdef PUBLISH_THREAD ROS_INFO_STREAM("[MRT_ROS_Interface::launchNodes] Publishing MRT messages on a separate thread."); #endif - ROS_INFO_STREAM("[MRT_ROS_Interface::launchNodes] MRT node is ready."); - + */ + statusModelModeMRTMsg_.data = true; statusModelModeMRTPublisher_.publish(statusModelModeMRTMsg_); diff --git a/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp b/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp index 678cacffa..52cd3296c 100644 --- a/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp +++ b/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp @@ -56,7 +56,7 @@ void RosReferenceManager::subscribe(ros::NodeHandle& nodeHandle) auto modeSchedule = ros_msg_conversions::readModeScheduleMsg(*msg); referenceManagerPtr_->setModeSchedule(std::move(modeSchedule)); }; - modeScheduleSubscriber_ = nodeHandle.subscribe(topicPrefix_ + "_mode_schedule", 1, modeScheduleCallback); + modeScheduleSubscriber_ = nodeHandle.subscribe(topicPrefix_ + "mode_schedule", 1, modeScheduleCallback); // TargetTrajectories auto targetTrajectoriesCallback = [this](const ocs2_msgs::mpc_target_trajectories::ConstPtr& msg) @@ -93,7 +93,7 @@ void RosReferenceManager::subscribe(ros::NodeHandle& nodeHandle) //std::cout << "[RosReferenceManager::subscribe::targetTrajectoriesCallback] END" << std::endl; //std::cout << "" << std::endl; }; - targetTrajectoriesSubscriber_ = nodeHandle.subscribe(topicPrefix_ + "_mpc_target", 1, targetTrajectoriesCallback); + targetTrajectoriesSubscriber_ = nodeHandle.subscribe(topicPrefix_ + "mpc_target", 1, targetTrajectoriesCallback); // ModelMode /*