diff --git a/docs/first-steps.rst b/docs/first-steps.rst index 6007cb47..91020cd5 100644 --- a/docs/first-steps.rst +++ b/docs/first-steps.rst @@ -58,9 +58,17 @@ published sensor topics. Move it ------------ -If you are running MVSim as a ROS node, you can launch any standard teleoperation node +If you are running MVSim as a ROS node, you can launch any standard teleoperation node and send motion commands to ``/cmd_vel`` as with any other robot or simulator. +If you want to use the teleop panel in rviz2, please install `visualization_tutorials `_. + +.. code-block:: bash + + cd ros2_ws/src + git clone -b ros2 https://github.com/ros-visualization/visualization_tutorials + colcon build --symlink-install + Additionally, MVSim allows you to **move the robot directly using the keyboard or a joystick**. Make sure of giving the focus to the MVSim window first, then use these keys: @@ -68,7 +76,7 @@ then use these keys: - ``w/s`` to increase/decrease the PI controller setpoint linear speed, and - ``a/d`` to change the corresponding angular speed, that is, rotate to the left and right. - Use the spacebar as a brake. -- In worlds with more than one robot, select the active robot by pressing the numeric +- In worlds with more than one robot, select the active robot by pressing the numeric keys ``1``, ``2``, etc. All the details on **keyboard and joystick-based control** are listed `here `_. diff --git a/formatter.sh b/formatter.sh index 06a0718b..dfa34ec9 100755 --- a/formatter.sh +++ b/formatter.sh @@ -1,2 +1,2 @@ # formatter.sh -find mvsim_node_src/ -iname *.h -o -iname *.hpp -o -iname *.cpp -o -iname *.c | xargs clang-format-14 -i +find modules/ mvsim_node_src/ mvsim_tutorial/cpp/ mvsim-cli/ tests/ -iname *.h -o -iname *.hpp -o -iname *.cpp -o -iname *.c | xargs clang-format-14 -i diff --git a/modules/comms/python/generated-sources-pybind/mvsim/Comms/Client.cpp b/modules/comms/python/generated-sources-pybind/mvsim/Comms/Client.cpp index 6984553a..b2ceb9d3 100644 --- a/modules/comms/python/generated-sources-pybind/mvsim/Comms/Client.cpp +++ b/modules/comms/python/generated-sources-pybind/mvsim/Comms/Client.cpp @@ -18,8 +18,7 @@ PYBIND11_DECLARE_HOLDER_TYPE(T, T*) PYBIND11_MAKE_OPAQUE(std::shared_ptr) #endif -void bind_mvsim_Comms_Client( - std::function& M) +void bind_mvsim_Comms_Client(std::function& M) { { // mvsim::Client file:mvsim/Comms/Client.h line:48 pybind11::class_> cl( @@ -39,45 +38,39 @@ void bind_mvsim_Comms_Client( cl.def(pybind11::init(), pybind11::arg("nodeName")); cl.def( - "setName", - (void (mvsim::Client::*)(const std::string&)) & - mvsim::Client::setName, + "setName", (void(mvsim::Client::*)(const std::string&)) & mvsim::Client::setName, "@{ \n\nC++: mvsim::Client::setName(const std::string &) --> void", pybind11::arg("nodeName")); cl.def( "serverHostAddress", - (const std::string& (mvsim::Client::*)() const) & - mvsim::Client::serverHostAddress, + (const std::string& (mvsim::Client::*)() const) & mvsim::Client::serverHostAddress, "C++: mvsim::Client::serverHostAddress() const --> const " "std::string &", pybind11::return_value_policy::automatic); cl.def( "serverHostAddress", - (void (mvsim::Client::*)(const std::string&)) & - mvsim::Client::serverHostAddress, + (void(mvsim::Client::*)(const std::string&)) & mvsim::Client::serverHostAddress, "C++: mvsim::Client::serverHostAddress(const std::string &) --> " "void", pybind11::arg("serverIpOrAddressName")); cl.def( - "connect", (void (mvsim::Client::*)()) & mvsim::Client::connect, + "connect", (void(mvsim::Client::*)()) & mvsim::Client::connect, "Connects to the server in a parallel thread.\n Default server " "address is `localhost`, can be changed with\n " "serverHostAddress().\n\nC++: mvsim::Client::connect() --> void"); cl.def( - "connected", - (bool (mvsim::Client::*)() const) & mvsim::Client::connected, + "connected", (bool(mvsim::Client::*)() const) & mvsim::Client::connected, "Whether the client is correctly connected to the server. \n\nC++: " "mvsim::Client::connected() const --> bool"); cl.def( - "shutdown", (void (mvsim::Client::*)()) & mvsim::Client::shutdown, + "shutdown", (void(mvsim::Client::*)()) & mvsim::Client::shutdown, "Shutdowns the communication thread. Blocks until the thread is " "stopped.\n There is no need to manually call this method, it is " "called upon\n destruction. \n\nC++: mvsim::Client::shutdown() --> " "void"); cl.def( "callService", - (std::string(mvsim::Client::*)( - const std::string&, const std::string&)) & + (std::string(mvsim::Client::*)(const std::string&, const std::string&)) & mvsim::Client::callService, "Overload for python wrapper\n\nC++: " "mvsim::Client::callService(const std::string &, const std::string " @@ -85,13 +78,12 @@ void bind_mvsim_Comms_Client( pybind11::arg("serviceName"), pybind11::arg("inputSerializedMsg")); cl.def( "subscribeTopic", - (void (mvsim::Client::*)( + (void(mvsim::Client::*)( const std::string&, const class std::function>&)>&)) & + unsigned char, class std::allocator>&)>&)) & mvsim::Client::subscribeTopic, "Overload for python wrapper (callback accepts " "bytes-string)\n\nC++: mvsim::Client::subscribeTopic(const " @@ -100,28 +92,22 @@ void bind_mvsim_Comms_Client( "std::allocator > &)> &) --> void", pybind11::arg("topicName"), pybind11::arg("callback")); cl.def( - "enable_profiler", - (void (mvsim::Client::*)(bool)) & mvsim::Client::enable_profiler, - "C++: mvsim::Client::enable_profiler(bool) --> void", - pybind11::arg("enable")); + "enable_profiler", (void(mvsim::Client::*)(bool)) & mvsim::Client::enable_profiler, + "C++: mvsim::Client::enable_profiler(bool) --> void", pybind11::arg("enable")); { // mvsim::Client::InfoPerNode file:mvsim/Comms/Client.h line:113 auto& enclosing_class = cl; pybind11::class_< - mvsim::Client::InfoPerNode, - std::shared_ptr> + mvsim::Client::InfoPerNode, std::shared_ptr> cl(enclosing_class, "InfoPerNode", ""); - cl.def(pybind11::init( - []() { return new mvsim::Client::InfoPerNode(); })); - cl.def(pybind11::init([](mvsim::Client::InfoPerNode const& o) { - return new mvsim::Client::InfoPerNode(o); - })); + cl.def(pybind11::init([]() { return new mvsim::Client::InfoPerNode(); })); + cl.def(pybind11::init([](mvsim::Client::InfoPerNode const& o) + { return new mvsim::Client::InfoPerNode(o); })); cl.def_readwrite("name", &mvsim::Client::InfoPerNode::name); cl.def( "assign", (struct mvsim::Client::InfoPerNode & - (mvsim::Client::InfoPerNode::*)(const struct mvsim::Client:: - InfoPerNode&)) & + (mvsim::Client::InfoPerNode::*)(const struct mvsim::Client::InfoPerNode&)) & mvsim::Client::InfoPerNode::operator=, "C++: mvsim::Client::InfoPerNode::operator=(const struct " "mvsim::Client::InfoPerNode &) --> struct " @@ -132,25 +118,19 @@ void bind_mvsim_Comms_Client( { // mvsim::Client::InfoPerTopic file:mvsim/Comms/Client.h line:119 auto& enclosing_class = cl; pybind11::class_< - mvsim::Client::InfoPerTopic, - std::shared_ptr> + mvsim::Client::InfoPerTopic, std::shared_ptr> cl(enclosing_class, "InfoPerTopic", ""); - cl.def(pybind11::init( - []() { return new mvsim::Client::InfoPerTopic(); })); - cl.def(pybind11::init([](mvsim::Client::InfoPerTopic const& o) { - return new mvsim::Client::InfoPerTopic(o); - })); + cl.def(pybind11::init([]() { return new mvsim::Client::InfoPerTopic(); })); + cl.def(pybind11::init([](mvsim::Client::InfoPerTopic const& o) + { return new mvsim::Client::InfoPerTopic(o); })); cl.def_readwrite("name", &mvsim::Client::InfoPerTopic::name); cl.def_readwrite("type", &mvsim::Client::InfoPerTopic::type); - cl.def_readwrite( - "endpoints", &mvsim::Client::InfoPerTopic::endpoints); - cl.def_readwrite( - "publishers", &mvsim::Client::InfoPerTopic::publishers); + cl.def_readwrite("endpoints", &mvsim::Client::InfoPerTopic::endpoints); + cl.def_readwrite("publishers", &mvsim::Client::InfoPerTopic::publishers); cl.def( "assign", (struct mvsim::Client::InfoPerTopic & - (mvsim::Client::InfoPerTopic::*)(const struct mvsim::Client:: - InfoPerTopic&)) & + (mvsim::Client::InfoPerTopic::*)(const struct mvsim::Client::InfoPerTopic&)) & mvsim::Client::InfoPerTopic::operator=, "C++: mvsim::Client::InfoPerTopic::operator=(const struct " "mvsim::Client::InfoPerTopic &) --> struct " diff --git a/modules/comms/python/generated-sources-pybind/mvsim/Comms/common.cpp b/modules/comms/python/generated-sources-pybind/mvsim/Comms/common.cpp index 49346f5a..dfc010d6 100644 --- a/modules/comms/python/generated-sources-pybind/mvsim/Comms/common.cpp +++ b/modules/comms/python/generated-sources-pybind/mvsim/Comms/common.cpp @@ -15,8 +15,7 @@ PYBIND11_MAKE_OPAQUE(std::shared_ptr) #endif // mvsim::UnexpectedMessageException file:mvsim/Comms/common.h line:49 -struct PyCallBack_mvsim_UnexpectedMessageException - : public mvsim::UnexpectedMessageException +struct PyCallBack_mvsim_UnexpectedMessageException : public mvsim::UnexpectedMessageException { using mvsim::UnexpectedMessageException::UnexpectedMessageException; @@ -24,14 +23,11 @@ struct PyCallBack_mvsim_UnexpectedMessageException { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload( - static_cast(this), - "what"); + static_cast(this), "what"); if (overload) { - auto o = - overload.operator()(); - if (pybind11::detail::cast_is_temporary_value_reference< - const char*>::value) + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { // pybind11 <=2.4: overload_caster_t, otherwise: override_caster_t #if (PYBIND11_MAJOR_VERSION == 2 && PYBIND11_MINOR_VERSION <= 4) @@ -39,8 +35,7 @@ struct PyCallBack_mvsim_UnexpectedMessageException #else static pybind11::detail::override_caster_t caster; #endif - return pybind11::detail::cast_ref( - std::move(o), caster); + return pybind11::detail::cast_ref(std::move(o), caster); } else return pybind11::detail::cast_safe(std::move(o)); @@ -49,30 +44,24 @@ struct PyCallBack_mvsim_UnexpectedMessageException } }; -void bind_mvsim_Comms_common( - std::function& M) +void bind_mvsim_Comms_common(std::function& M) { { // mvsim::UnexpectedMessageException file:mvsim/Comms/common.h line:49 pybind11::class_< - mvsim::UnexpectedMessageException, - std::shared_ptr, + mvsim::UnexpectedMessageException, std::shared_ptr, PyCallBack_mvsim_UnexpectedMessageException, std::runtime_error> cl(M("mvsim"), "UnexpectedMessageException", ""); cl.def(pybind11::init(), pybind11::arg("reason")); - cl.def(pybind11::init( - [](PyCallBack_mvsim_UnexpectedMessageException const& o) { - return new PyCallBack_mvsim_UnexpectedMessageException(o); - })); - cl.def(pybind11::init([](mvsim::UnexpectedMessageException const& o) { - return new mvsim::UnexpectedMessageException(o); - })); + cl.def(pybind11::init([](PyCallBack_mvsim_UnexpectedMessageException const& o) + { return new PyCallBack_mvsim_UnexpectedMessageException(o); })); + cl.def(pybind11::init([](mvsim::UnexpectedMessageException const& o) + { return new mvsim::UnexpectedMessageException(o); })); cl.def( "assign", (class mvsim::UnexpectedMessageException & - (mvsim:: - UnexpectedMessageException::*)(const class mvsim:: - UnexpectedMessageException&)) & + (mvsim::UnexpectedMessageException::*)(const class mvsim:: + UnexpectedMessageException&)) & mvsim::UnexpectedMessageException::operator=, "C++: mvsim::UnexpectedMessageException::operator=(const class " "mvsim::UnexpectedMessageException &) --> class " diff --git a/modules/comms/python/generated-sources-pybind/pymvsim_comms.cpp b/modules/comms/python/generated-sources-pybind/pymvsim_comms.cpp index 3112c6e5..67222138 100644 --- a/modules/comms/python/generated-sources-pybind/pymvsim_comms.cpp +++ b/modules/comms/python/generated-sources-pybind/pymvsim_comms.cpp @@ -9,26 +9,23 @@ typedef std::function ModuleGetter; -void bind_std_exception( - std::function& M); -void bind_std_stdexcept( - std::function& M); -void bind_mvsim_Comms_common( - std::function& M); -void bind_mvsim_Comms_Client( - std::function& M); +void bind_std_exception(std::function& M); +void bind_std_stdexcept(std::function& M); +void bind_mvsim_Comms_common(std::function& M); +void bind_mvsim_Comms_Client(std::function& M); PYBIND11_MODULE(pymvsim_comms, root_module) { root_module.doc() = "pymvsim_comms module"; std::map modules; - ModuleGetter M = [&](std::string const& namespace_) -> pybind11::module& { + ModuleGetter M = [&](std::string const& namespace_) -> pybind11::module& + { auto it = modules.find(namespace_); if (it == modules.end()) throw std::runtime_error( - "Attempt to access pybind11::module for namespace " + - namespace_ + " before it was created!!!"); + "Attempt to access pybind11::module for namespace " + namespace_ + + " before it was created!!!"); return it->second; }; @@ -39,14 +36,15 @@ PYBIND11_MODULE(pymvsim_comms, root_module) "global", }; - auto mangle_namespace_name([](std::string const& ns) -> std::string { - if (std::find( - reserved_python_words.begin(), reserved_python_words.end(), - ns) == reserved_python_words.end()) - return ns; - else - return ns + '_'; - }); + auto mangle_namespace_name( + [](std::string const& ns) -> std::string + { + if (std::find(reserved_python_words.begin(), reserved_python_words.end(), ns) == + reserved_python_words.end()) + return ns; + else + return ns + '_'; + }); std::vector> sub_modules{ {"", "mvsim"}, @@ -56,8 +54,7 @@ PYBIND11_MODULE(pymvsim_comms, root_module) modules[p.first.size() ? p.first + "::" + p.second : p.second] = modules[p.first].def_submodule( mangle_namespace_name(p.second).c_str(), - ("Bindings for " + p.first + "::" + p.second + " namespace") - .c_str()); + ("Bindings for " + p.first + "::" + p.second + " namespace").c_str()); // pybind11::class_>(M(""), "_encapsulated_data_"); diff --git a/modules/comms/python/generated-sources-pybind/std/exception.cpp b/modules/comms/python/generated-sources-pybind/std/exception.cpp index f77be28e..ebac1bfc 100644 --- a/modules/comms/python/generated-sources-pybind/std/exception.cpp +++ b/modules/comms/python/generated-sources-pybind/std/exception.cpp @@ -21,14 +21,12 @@ struct PyCallBack_std_exception : public std::exception const char* what() const throw() override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload( - static_cast(this), "what"); + pybind11::function overload = + pybind11::get_overload(static_cast(this), "what"); if (overload) { - auto o = - overload.operator()(); - if (pybind11::detail::cast_is_temporary_value_reference< - const char*>::value) + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { // pybind11 <=2.4: overload_caster_t, otherwise: override_caster_t #if (PYBIND11_MAJOR_VERSION == 2 && PYBIND11_MINOR_VERSION <= 4) @@ -36,8 +34,7 @@ struct PyCallBack_std_exception : public std::exception #else static pybind11::detail::override_caster_t caster; #endif - return pybind11::detail::cast_ref( - std::move(o), caster); + return pybind11::detail::cast_ref(std::move(o), caster); } else return pybind11::detail::cast_safe(std::move(o)); @@ -46,33 +43,26 @@ struct PyCallBack_std_exception : public std::exception } }; -void bind_std_exception( - std::function& M) +void bind_std_exception(std::function& M) { { // std::exception file:bits/exception.h line:61 - pybind11::class_< - std::exception, std::shared_ptr, - PyCallBack_std_exception> + pybind11::class_, PyCallBack_std_exception> cl(M("std"), "exception", ""); cl.def(pybind11::init( []() { return new std::exception(); }, []() { return new PyCallBack_std_exception(); })); - cl.def(pybind11::init([](PyCallBack_std_exception const& o) { - return new PyCallBack_std_exception(o); - })); - cl.def(pybind11::init( - [](std::exception const& o) { return new std::exception(o); })); + cl.def(pybind11::init([](PyCallBack_std_exception const& o) + { return new PyCallBack_std_exception(o); })); + cl.def(pybind11::init([](std::exception const& o) { return new std::exception(o); })); cl.def( "assign", - (class std::exception & - (std::exception::*)(const class std::exception&)) & + (class std::exception & (std::exception::*)(const class std::exception&)) & std::exception::operator=, "C++: std::exception::operator=(const class std::exception &) --> " "class std::exception &", pybind11::return_value_policy::automatic, pybind11::arg("")); cl.def( - "what", - (const char* (std::exception::*)() const) & std::exception::what, + "what", (const char* (std::exception::*)() const) & std::exception::what, "C++: std::exception::what() const --> const char *", pybind11::return_value_policy::automatic); } diff --git a/modules/comms/python/generated-sources-pybind/std/stdexcept.cpp b/modules/comms/python/generated-sources-pybind/std/stdexcept.cpp index 05357e2d..aa9b2d44 100644 --- a/modules/comms/python/generated-sources-pybind/std/stdexcept.cpp +++ b/modules/comms/python/generated-sources-pybind/std/stdexcept.cpp @@ -23,14 +23,12 @@ struct PyCallBack_std_runtime_error : public std::runtime_error const char* what() const throw() override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload( - static_cast(this), "what"); + pybind11::function overload = + pybind11::get_overload(static_cast(this), "what"); if (overload) { - auto o = - overload.operator()(); - if (pybind11::detail::cast_is_temporary_value_reference< - const char*>::value) + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { // pybind11 <=2.4: overload_caster_t, otherwise: override_caster_t #if (PYBIND11_MAJOR_VERSION == 2 && PYBIND11_MINOR_VERSION <= 4) @@ -38,8 +36,7 @@ struct PyCallBack_std_runtime_error : public std::runtime_error #else static pybind11::detail::override_caster_t caster; #endif - return pybind11::detail::cast_ref( - std::move(o), caster); + return pybind11::detail::cast_ref(std::move(o), caster); } else return pybind11::detail::cast_safe(std::move(o)); @@ -48,36 +45,30 @@ struct PyCallBack_std_runtime_error : public std::runtime_error } }; -void bind_std_stdexcept( - std::function& M) +void bind_std_stdexcept(std::function& M) { { // std::runtime_error file:stdexcept line:219 pybind11::class_< - std::runtime_error, std::shared_ptr, - PyCallBack_std_runtime_error, std::exception> + std::runtime_error, std::shared_ptr, PyCallBack_std_runtime_error, + std::exception> cl(M("std"), "runtime_error", ""); cl.def(pybind11::init(), pybind11::arg("__arg")); cl.def(pybind11::init(), pybind11::arg("")); - cl.def(pybind11::init([](PyCallBack_std_runtime_error const& o) { - return new PyCallBack_std_runtime_error(o); - })); - cl.def(pybind11::init([](std::runtime_error const& o) { - return new std::runtime_error(o); - })); + cl.def(pybind11::init([](PyCallBack_std_runtime_error const& o) + { return new PyCallBack_std_runtime_error(o); })); + cl.def( + pybind11::init([](std::runtime_error const& o) { return new std::runtime_error(o); })); cl.def( "assign", - (class std::runtime_error & - (std::runtime_error::*)(const class std::runtime_error&)) & + (class std::runtime_error & (std::runtime_error::*)(const class std::runtime_error&)) & std::runtime_error::operator=, "C++: std::runtime_error::operator=(const class std::runtime_error " "&) --> class std::runtime_error &", pybind11::return_value_policy::automatic, pybind11::arg("")); cl.def( - "what", - (const char* (std::runtime_error::*)() const) & - std::runtime_error::what, + "what", (const char* (std::runtime_error::*)() const) & std::runtime_error::what, "C++: std::runtime_error::what() const --> const char *", pybind11::return_value_policy::automatic); } diff --git a/mvsim-cli/mvsim-cli-launch.cpp b/mvsim-cli/mvsim-cli-launch.cpp index 4116bbe2..8f0dac14 100644 --- a/mvsim-cli/mvsim-cli-launch.cpp +++ b/mvsim-cli/mvsim-cli-launch.cpp @@ -103,8 +103,7 @@ void mvsim_install_signal_handler() // returns log strings std::string mvsim_launch_handle_teleop( - const mvsim::World::TGUIKeyEvent keyevent, - const std::optional& js) + const mvsim::World::TGUIKeyEvent keyevent, const std::optional& js) { using namespace mvsim; @@ -112,8 +111,7 @@ std::string mvsim_launch_handle_teleop( const World::VehicleList& vehs = app->world.getListOfVehicles(); txt2gui_tmp += mrpt::format( - "Selected vehicle: %u/%u\n", - static_cast(app->teleopIdxVeh + 1), + "Selected vehicle: %u/%u\n", static_cast(app->teleopIdxVeh + 1), static_cast(vehs.size())); if (vehs.size() > app->teleopIdxVeh) { @@ -123,26 +121,23 @@ std::string mvsim_launch_handle_teleop( // Get speed: ground truth { - const mrpt::math::TTwist2D& vel = - it_veh->second->getVelocityLocal(); + const mrpt::math::TTwist2D& vel = it_veh->second->getVelocityLocal(); txt2gui_tmp += mrpt::format( - "gt. vel: lx=%7.03f, ly=%7.03f, w= %7.03fdeg/s\n", vel.vx, - vel.vy, mrpt::RAD2DEG(vel.omega)); + "gt. vel: lx=%7.03f, ly=%7.03f, w= %7.03fdeg/s\n", vel.vx, vel.vy, + mrpt::RAD2DEG(vel.omega)); } // Get speed: ground truth { - const mrpt::math::TTwist2D& vel = - it_veh->second->getVelocityLocalOdoEstimate(); + const mrpt::math::TTwist2D& vel = it_veh->second->getVelocityLocalOdoEstimate(); txt2gui_tmp += mrpt::format( - "odo vel: lx=%7.03f, ly=%7.03f, w= %7.03fdeg/s\n", vel.vx, - vel.vy, mrpt::RAD2DEG(vel.omega)); + "odo vel: lx=%7.03f, ly=%7.03f, w= %7.03fdeg/s\n", vel.vx, vel.vy, + mrpt::RAD2DEG(vel.omega)); } // Generic teleoperation interface for any controller that // supports it: { - ControllerBaseInterface* controller = - it_veh->second->getControllerInterface(); + ControllerBaseInterface* controller = it_veh->second->getControllerInterface(); ControllerBaseInterface::TeleopInput teleop_in; ControllerBaseInterface::TeleopOutput teleop_out; teleop_in.keycode = keyevent.keycode; @@ -182,22 +177,19 @@ Available options: // Handle CTRL+C: mvsim_install_signal_handler(); - const auto verbosityLevel = - mrpt::typemeta::TEnumType::name2value( - cli->argVerbosity.getValue()); + const auto verbosityLevel = mrpt::typemeta::TEnumType::name2value( + cli->argVerbosity.getValue()); if (verbosityLevel <= mrpt::system::LVL_INFO) { - mrpt::system::consoleColorAndStyle( - mrpt::system::ConsoleForegroundColor::BRIGHT_YELLOW); + mrpt::system::consoleColorAndStyle(mrpt::system::ConsoleForegroundColor::BRIGHT_YELLOW); std::cout // << "\n" << "====================================================\n" << " MVSIM simulator running. Press CTRL+C to end. \n" << "====================================================\n" << "\n"; - mrpt::system::consoleColorAndStyle( - mrpt::system::ConsoleForegroundColor::DEFAULT); + mrpt::system::consoleColorAndStyle(mrpt::system::ConsoleForegroundColor::DEFAULT); } const auto sXMLfilename = unlabeledArgs.at(1); @@ -207,8 +199,7 @@ Available options: app->world.setMinLoggingLevel(verbosityLevel); // CLI flags: - if (cli->argFullProfiler.isSet()) - app->world.getTimeLogger().enableKeepWholeHistory(); + if (cli->argFullProfiler.isSet()) app->world.getTimeLogger().enableKeepWholeHistory(); if (cli->argHeadless.isSet()) app->world.headless(true); @@ -237,14 +228,12 @@ Available options: if (!cli->argHeadless.isSet()) { // regular GUI: - app->thGUI = std::thread( - &mvsim_server_thread_update_GUI, std::ref(app->thread_params)); + app->thGUI = std::thread(&mvsim_server_thread_update_GUI, std::ref(app->thread_params)); } else { // headless thread for off-screen rendering sensors: - app->thGUI = std::thread( - &mvsim_server_thread_headless, std::ref(app->thread_params)); + app->thGUI = std::thread(&mvsim_server_thread_headless, std::ref(app->thread_params)); } // Run simulation: @@ -261,16 +250,14 @@ Available options: // ============================================================ // Compute how much time has passed to simulate in real-time: double tNew = mrpt::Clock::nowDouble(); - double incrTime = - rtFactor * (tNew - tAbsInit) - app->world.get_simul_time(); - int incrTimeSteps = static_cast( - std::floor(incrTime / app->world.get_simul_timestep())); + double incrTime = rtFactor * (tNew - tAbsInit) - app->world.get_simul_time(); + int incrTimeSteps = + static_cast(std::floor(incrTime / app->world.get_simul_timestep())); // Simulate: if (incrTimeSteps > 0) { - app->world.run_simulation( - incrTimeSteps * app->world.get_simul_timestep()); + app->world.run_simulation(incrTimeSteps * app->world.get_simul_timestep()); } std::this_thread::sleep_for(std::chrono::milliseconds(10)); @@ -345,8 +332,7 @@ void mvsim_server_thread_update_GUI(TThreadParams& thread_params) } catch (const std::exception& e) { - std::cerr << "[mvsim_server_thread_update_GUI] Exception: " << e.what() - << std::endl; + std::cerr << "[mvsim_server_thread_update_GUI] Exception: " << e.what() << std::endl; } } @@ -355,13 +341,12 @@ void mvsim_server_thread_headless(TThreadParams& thread_params) try { ASSERT_(thread_params.world); - while (!thread_params.isClosing() && - !thread_params.world->simulator_must_close()) + while (!thread_params.isClosing() && !thread_params.world->simulator_must_close()) { thread_params.world->internalGraphicsLoopTasksForSimulation(); - std::this_thread::sleep_for(std::chrono::microseconds(mrpt::round( - thread_params.world->get_simul_timestep() * 1000000))); + std::this_thread::sleep_for(std::chrono::microseconds( + mrpt::round(thread_params.world->get_simul_timestep() * 1000000))); } // in case we are here due to simulator_must_close() @@ -369,7 +354,6 @@ void mvsim_server_thread_headless(TThreadParams& thread_params) } catch (const std::exception& e) { - std::cerr << "[mvsim_server_thread_update_GUI] Exception: " << e.what() - << std::endl; + std::cerr << "[mvsim_server_thread_update_GUI] Exception: " << e.what() << std::endl; } } diff --git a/mvsim-cli/mvsim-cli-main.cpp b/mvsim-cli/mvsim-cli-main.cpp index 2553e62d..68788a92 100644 --- a/mvsim-cli/mvsim-cli-main.cpp +++ b/mvsim-cli/mvsim-cli-main.cpp @@ -24,18 +24,15 @@ std::unique_ptr cli; const std::map cliCommands = { - {"help", cmd_t(&printListCommands)}, - {"server", cmd_t(&launchStandAloneServer)}, - {"launch", cmd_t(&launchSimulation)}, - {"node", cmd_t(&commandNode)}, + {"help", cmd_t(&printListCommands)}, {"server", cmd_t(&launchStandAloneServer)}, + {"launch", cmd_t(&launchSimulation)}, {"node", cmd_t(&commandNode)}, {"topic", cmd_t(&commandTopic)}, }; void setConsoleErrorColor() { #if MRPT_VERSION >= 0x233 - mrpt::system::consoleColorAndStyle( - mrpt::system::ConsoleForegroundColor::RED); + mrpt::system::consoleColorAndStyle(mrpt::system::ConsoleForegroundColor::RED); #else mrpt::system::setConsoleColor(mrpt::system::CONCOL_RED); #endif @@ -44,8 +41,7 @@ void setConsoleErrorColor() void setConsoleNormalColor() { #if MRPT_VERSION >= 0x233 - mrpt::system::consoleColorAndStyle( - mrpt::system::ConsoleForegroundColor::DEFAULT); + mrpt::system::consoleColorAndStyle(mrpt::system::ConsoleForegroundColor::DEFAULT); #else mrpt::system::setConsoleColor(mrpt::system::CONCOL_NORMAL); #endif @@ -71,8 +67,7 @@ int main(int argc, char** argv) // Take first unlabeled argument: std::string command; - if (const auto& lst = cli->argCmd.getValue(); !lst.empty()) - command = lst.at(0); + if (const auto& lst = cli->argCmd.getValue(); !lst.empty()) command = lst.at(0); // Look up command in table: auto itCmd = cliCommands.find(command); diff --git a/mvsim-cli/mvsim-cli-node.cpp b/mvsim-cli/mvsim-cli-node.cpp index be7986f9..86c722b1 100644 --- a/mvsim-cli/mvsim-cli-node.cpp +++ b/mvsim-cli/mvsim-cli-node.cpp @@ -38,9 +38,8 @@ int nodeList() { mvsim::Client client; - client.setMinLoggingLevel( - mrpt::typemeta::TEnumType::name2value( - cli->argVerbosity.getValue())); + client.setMinLoggingLevel(mrpt::typemeta::TEnumType::name2value( + cli->argVerbosity.getValue())); std::cout << "# Connecting to server...\n"; client.connect(); diff --git a/mvsim-cli/mvsim-cli-server.cpp b/mvsim-cli/mvsim-cli-server.cpp index 96cc3f25..a1b72775 100644 --- a/mvsim-cli/mvsim-cli-server.cpp +++ b/mvsim-cli/mvsim-cli-server.cpp @@ -24,9 +24,8 @@ void commonLaunchServer() if (cli->argPort.isSet()) server->listenningPort(cli->argPort.getValue()); - server->setMinLoggingLevel( - mrpt::typemeta::TEnumType::name2value( - cli->argVerbosity.getValue())); + server->setMinLoggingLevel(mrpt::typemeta::TEnumType::name2value( + cli->argVerbosity.getValue())); server->start(); } diff --git a/mvsim-cli/mvsim-cli-topic.cpp b/mvsim-cli/mvsim-cli-topic.cpp index 78aa7221..2be5b4b8 100644 --- a/mvsim-cli/mvsim-cli-topic.cpp +++ b/mvsim-cli/mvsim-cli-topic.cpp @@ -32,8 +32,7 @@ int commandTopic() { const auto& lstCmds = cli->argCmd.getValue(); if (cli->argHelp.isSet()) return printCommandsTopic(false); - if (lstCmds.size() != 2 && lstCmds.size() != 3) - return printCommandsTopic(true); + if (lstCmds.size() != 2 && lstCmds.size() != 3) return printCommandsTopic(true); // Take second unlabeled argument: const std::string subcommand = lstCmds.at(1); @@ -51,9 +50,8 @@ int topicList() { mvsim::Client client; - client.setMinLoggingLevel( - mrpt::typemeta::TEnumType::name2value( - cli->argVerbosity.getValue())); + client.setMinLoggingLevel(mrpt::typemeta::TEnumType::name2value( + cli->argVerbosity.getValue())); std::cout << "# Connecting to server...\n"; client.connect(); @@ -76,8 +74,7 @@ int topicList() for (size_t i = 0; i < n.endpoints.size(); i++) { std::cout << " - endpoint: \"" << n.endpoints[i] << "\"\n" - << " - publisherNode: \"" << n.publishers[i] - << "\"\n"; + << " - publisherNode: \"" << n.publishers[i] << "\"\n"; } } else @@ -115,8 +112,7 @@ static void echo_ObservationLidar2D(const std::string& data) static void callbackSubscribeTopicGeneric(const zmq::message_t& msg) { - const auto [typeName, serializedData] = - mvsim::internal::parseMessageToParts(msg); + const auto [typeName, serializedData] = mvsim::internal::parseMessageToParts(msg); std::cout << "[" << mrpt::system::dateTimeLocalToString(mrpt::Clock::now()) << "] Received data : \n"; std::cout << " - typeName: " << typeName << "\n"; @@ -134,9 +130,8 @@ int topicEcho() { mvsim::Client client; - client.setMinLoggingLevel( - mrpt::typemeta::TEnumType::name2value( - cli->argVerbosity.getValue())); + client.setMinLoggingLevel(mrpt::typemeta::TEnumType::name2value( + cli->argVerbosity.getValue())); const auto& lstCmds = cli->argCmd.getValue(); if (lstCmds.size() != 3) return printCommandsTopic(true); @@ -147,8 +142,7 @@ int topicEcho() client.connect(); std::cout << "# Connected.\n"; - std::cout << "# Subscribing to topic '" << topicName - << "'. Press CTRL+C to stop.\n"; + std::cout << "# Subscribing to topic '" << topicName << "'. Press CTRL+C to stop.\n"; client.subscribe_topic_raw(topicName, &callbackSubscribeTopicGeneric); // loop until user does a CTRL+C @@ -163,9 +157,8 @@ int topicHz() { mvsim::Client client; - client.setMinLoggingLevel( - mrpt::typemeta::TEnumType::name2value( - cli->argVerbosity.getValue())); + client.setMinLoggingLevel(mrpt::typemeta::TEnumType::name2value( + cli->argVerbosity.getValue())); const auto& lstCmds = cli->argCmd.getValue(); if (lstCmds.size() != 3) return printCommandsTopic(true); @@ -178,23 +171,26 @@ int topicHz() const double WAIT_SECONDS = 5.0; - std::cout << "# Subscribing to topic '" << topicName - << "'. Will listen for " << WAIT_SECONDS << " seconds...\n"; + std::cout << "# Subscribing to topic '" << topicName << "'. Will listen for " << WAIT_SECONDS + << " seconds...\n"; int numMsgs = 0; std::optional lastMsgTim; std::vector measuredPeriods; - client.subscribe_topic_raw(topicName, [&](const zmq::message_t&) { - numMsgs++; - const double t = mrpt::Clock::nowDouble(); - if (lastMsgTim) + client.subscribe_topic_raw( + topicName, + [&](const zmq::message_t&) { - const double dt = t - lastMsgTim.value(); - measuredPeriods.push_back(dt); - } - lastMsgTim = t; - }); + numMsgs++; + const double t = mrpt::Clock::nowDouble(); + if (lastMsgTim) + { + const double dt = t - lastMsgTim.value(); + measuredPeriods.push_back(dt); + } + lastMsgTim = t; + }); std::this_thread::sleep_for( std::chrono::milliseconds(static_cast(WAIT_SECONDS * 1000))); @@ -210,9 +206,8 @@ int topicHz() double periodMean = 0, periodStd = 0; mrpt::math::meanAndStd(measuredPeriods, periodMean, periodStd); - std::cout << "- MeanPeriod: " << periodMean - << " # [sec] 1/T = " << 1.0 / periodMean << " Hz" - << std::endl; + std::cout << "- MeanPeriod: " << periodMean << " # [sec] 1/T = " << 1.0 / periodMean + << " Hz" << std::endl; std::cout << "- PeriodStdDev: " << periodStd << " # [sec]" << std::endl; @@ -226,8 +221,7 @@ int topicHz() double tMean, tLow, tHigh; mrpt::math::CVectorDouble x; - for (size_t i = 0; i < measuredPeriods.size(); i++) - x.push_back(measuredPeriods[i]); + for (size_t i = 0; i < measuredPeriods.size(); i++) x.push_back(measuredPeriods[i]); mrpt::math::confidenceIntervals(x, tMean, tLow, tHigh, conf, 100); diff --git a/mvsim-cli/mvsim-cli.h b/mvsim-cli/mvsim-cli.h index e1dec2ca..de1962e1 100644 --- a/mvsim-cli/mvsim-cli.h +++ b/mvsim-cli/mvsim-cli.h @@ -22,13 +22,10 @@ struct cli_flags TCLAP::CmdLine cmd{"mvsim", ' ', "version", false /* no --help */}; TCLAP::UnlabeledMultiArg argCmd{ - "command", "Command to run. Run 'mvsim help' to list commands.", false, - "", cmd}; + "command", "Command to run. Run 'mvsim help' to list commands.", false, "", cmd}; TCLAP::ValueArg argVerbosity{ - "v", "verbose", "Verbosity level", - false, "INFO", "ERROR|WARN|INFO|DEBUG", - cmd}; + "v", "verbose", "Verbosity level", false, "INFO", "ERROR|WARN|INFO|DEBUG", cmd}; TCLAP::SwitchArg argFullProfiler{ "", "full-profiler", @@ -37,26 +34,16 @@ struct cli_flags "program.", cmd}; - TCLAP::SwitchArg argHeadless{ - "", "headless", "Runs the simulator without any GUI window.", cmd}; + TCLAP::SwitchArg argHeadless{"", "headless", "Runs the simulator without any GUI window.", cmd}; - TCLAP::SwitchArg argDetails{ - "", "details", "Shows details in the specified subcommand", cmd}; + TCLAP::SwitchArg argDetails{"", "details", "Shows details in the specified subcommand", cmd}; - TCLAP::SwitchArg argVersion{ - "", "version", "Shows program version and exits", cmd}; + TCLAP::SwitchArg argVersion{"", "version", "Shows program version and exits", cmd}; - TCLAP::SwitchArg argHelp{ - "h", "help", "Shows more detailed help for command", cmd}; + TCLAP::SwitchArg argHelp{"h", "help", "Shows more detailed help for command", cmd}; TCLAP::ValueArg argPort{ - "p", - "port", - "TCP port to listen at", - false, - mvsim::MVSIM_PORTNO_MAIN_REP, - "TCP port", - cmd}; + "p", "port", "TCP port to listen at", false, mvsim::MVSIM_PORTNO_MAIN_REP, "TCP port", cmd}; TCLAP::ValueArg argRealTimeFactor{ "", diff --git a/mvsim_node_src/include/mvsim/mvsim_node_core.h b/mvsim_node_src/include/mvsim/mvsim_node_core.h index a244a53e..3fb90fe0 100644 --- a/mvsim_node_src/include/mvsim/mvsim_node_core.h +++ b/mvsim_node_src/include/mvsim/mvsim_node_core.h @@ -174,30 +174,24 @@ class MVSimNode #endif // === ROS Publishers ==== - /// used for simul_map publication #if PACKAGE_ROS_VERSION == 1 - mvsim_node::shared_ptr pub_map_ros_, pub_map_metadata_; // mvsim_node::shared_ptr pub_clock_; #else - rclcpp::Publisher::SharedPtr pub_map_ros_; - rclcpp::Publisher::SharedPtr pub_map_metadata_; rclcpp::TimeSource ts_{n_}; rclcpp::Clock::SharedPtr clock_; #endif -#if PACKAGE_ROS_VERSION == 1 - tf2_ros::TransformBroadcaster tf_br_; //!< Use to send data to TF - tf2_ros::StaticTransformBroadcaster static_tf_br_; -#else - tf2_ros::TransformBroadcaster tf_br_{n_}; //!< Use to send data to TF - tf2_ros::StaticTransformBroadcaster static_tf_br_{n_}; -#endif - struct TPubSubPerVehicle { #if PACKAGE_ROS_VERSION == 1 mvsim_node::shared_ptr sub_cmd_vel; //!< Subscribers vehicle's "cmd_vel" topic + + /// used for simul_map publication + mvsim_node::shared_ptr pub_map_ros; //!< Publisher of "simul_map" topic + mvsim_node::shared_ptr + pub_map_metadata; //!< Publisher of "simul_map_metadata" topic + mvsim_node::shared_ptr pub_odom; //!< Publisher of "odom" topic mvsim_node::shared_ptr pub_ground_truth; //!< "base_pose_ground_truth" topic @@ -221,6 +215,11 @@ class MVSimNode #else /// Subscribers vehicle's "cmd_vel" topic rclcpp::Subscription::SharedPtr sub_cmd_vel; + + /// used for simul_map publication + rclcpp::Publisher::SharedPtr pub_map_ros; + rclcpp::Publisher::SharedPtr pub_map_metadata; + /// Publisher of "odom" topic rclcpp::Publisher::SharedPtr pub_odom; /// "base_pose_ground_truth" topic @@ -320,16 +319,12 @@ class MVSimNode * one vehicle in the World, or "/" otherwise. */ std::string vehVarName(const std::string& sVarName, const mvsim::VehicleBase& veh) const; - void sendStaticTF( - const std::string& frame_id, const std::string& child_frame_id, const tf2::Transform& tx, - const ros_Time& stamp); - ros_Time myNow() const; double myNowSec() const; mrpt::system::CTimeLogger profiler_{true /*enabled*/, "mvsim_node"}; - void publishWorldElements(mvsim::WorldElementBase& obj); + void publishWorldElements(mvsim::WorldElementBase& obj, TPubSubPerVehicle& pubsubs); void publishVehicles(mvsim::VehicleBase& veh); void internalOn(const mvsim::VehicleBase& veh, const mrpt::obs::CObservation2DRangeScan& obs); diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp index 08e0f2ee..3a68efc8 100644 --- a/mvsim_node_src/mvsim_node.cpp +++ b/mvsim_node_src/mvsim_node.cpp @@ -189,17 +189,6 @@ MVSimNode::MVSimNode(rclcpp::Node::SharedPtr& n) // pub_clock_ = // mvsim_node::make_shared(n_.advertise("/clock", // 10)); - - pub_map_ros_ = mvsim_node::make_shared( - n_.advertise("simul_map", 1 /*queue len*/, true /*latch*/)); - pub_map_metadata_ = mvsim_node::make_shared( - n_.advertise("simul_map_metadata", 1 /*queue len*/, true /*latch*/)); -#else - rclcpp::QoS qosLatched(rclcpp::KeepLast(10)); - qosLatched.durability(rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); - - pub_map_ros_ = n_->create_publisher("simul_map", qosLatched); - pub_map_metadata_ = n_->create_publisher("simul_map_metadata", qosLatched); #endif #if PACKAGE_ROS_VERSION == 1 @@ -468,7 +457,7 @@ void MVSimNode::publishVehicles([[maybe_unused]] mvsim::VehicleBase& veh) // Visitor: World elements // ---------------------------------------- -void MVSimNode::publishWorldElements(mvsim::WorldElementBase& obj) +void MVSimNode::publishWorldElements(mvsim::WorldElementBase& obj, TPubSubPerVehicle& pubsubs) { // GridMaps -------------- static mrpt::system::CTicTac lastMapPublished; @@ -494,8 +483,8 @@ void MVSimNode::publishWorldElements(mvsim::WorldElementBase& obj) #endif ros_map.header.stamp = myNow(); - pub_map_ros_->publish(ros_map); - pub_map_metadata_->publish(ros_map.info); + pubsubs.pub_map_ros->publish(ros_map); + pubsubs.pub_map_metadata->publish(ros_map.info); } // end gridmap @@ -504,19 +493,6 @@ void MVSimNode::publishWorldElements(mvsim::WorldElementBase& obj) // ROS: Publish grid map for visualization purposes: void MVSimNode::notifyROSWorldIsUpdated() { -#if PACKAGE_ROS_VERSION == 2 - // In ROS1 latching works so we only need to do this once, here. - // In ROS2,latching doesn't work, we must re-publish on a regular basis... - static mrpt::system::CTicTac lastMapPublished; - if (lastMapPublished.Tac() > 2.0) - { - lastMapPublished.Tic(); - - mvsim_world_->runVisitorOnWorldElements([this](mvsim::WorldElementBase& obj) - { publishWorldElements(obj); }); - } -#endif - mvsim_world_->runVisitorOnVehicles([this](mvsim::VehicleBase& v) { publishVehicles(v); }); // Create subscribers & publishers for each vehicle's stuff: @@ -530,11 +506,23 @@ void MVSimNode::notifyROSWorldIsUpdated() mvsim::VehicleBase* veh = dynamic_cast(it->second.get()); if (!veh) continue; - initPubSubs(pubsub_vehicles_[idx], veh); - } + auto& pubsubs = pubsub_vehicles_[idx]; + + initPubSubs(pubsubs, veh); + +#if PACKAGE_ROS_VERSION == 2 + // In ROS1 latching works so we only need to do this once, here. + // In ROS2,latching doesn't work, we must re-publish on a regular basis... + static mrpt::system::CTicTac lastMapPublished; + if (lastMapPublished.Tac() > 2.0) + { + lastMapPublished.Tic(); - // Publish the static transform /world -> /map - // sendStaticTF("world", "map", tfIdentity_, myNow()); + mvsim_world_->runVisitorOnWorldElements([this, &pubsubs](mvsim::WorldElementBase& obj) + { publishWorldElements(obj, pubsubs); }); + } +#endif + } } ros_Time MVSimNode::myNow() const @@ -555,18 +543,6 @@ double MVSimNode::myNowSec() const #endif } -void MVSimNode::sendStaticTF( - const std::string& frame_id, const std::string& child_frame_id, const tf2::Transform& txf, - const ros_Time& stamp) -{ - Msg_TransformStamped tx; - tx.header.frame_id = frame_id; - tx.child_frame_id = child_frame_id; - tx.header.stamp = stamp; - tx.transform = tf2::toMsg(txf); - static_tf_br_.sendTransform(tx); -} - /** Initialize all pub/subs required for each vehicle, for the specific vehicle * \a veh */ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) @@ -583,6 +559,13 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) #endif #if PACKAGE_ROS_VERSION == 1 + // pub: /simul_map, /simul_map_metadata + pubsubs.pub_map_ros = mvsim_node::make_shared(n_.advertise( + vehVarName("simul_map", *veh), 1 /*queue len*/, true /*latch*/)); + pubsubs.pub_map_metadata = + mvsim_node::make_shared(n_.advertise( + vehVarName("simul_map_metadata", *veh), 1 /*queue len*/, true /*latch*/)); + // pub: /odom pubsubs.pub_odom = mvsim_node::make_shared( n_.advertise(vehVarName("odom", *veh), publisher_history_len_)); @@ -601,6 +584,15 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) pubsubs.pub_tf_static = mvsim_node::make_shared( n_.advertise(vehVarName("tf_static", *veh), publisher_history_len_)); #else + // pub: /simul_map, /simul_map_metadata + rclcpp::QoS qosLatched(rclcpp::KeepLast(10)); + qosLatched.durability(rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + + pubsubs.pub_map_ros = + n_->create_publisher(vehVarName("simul_map", *veh), qosLatched); + pubsubs.pub_map_metadata = + n_->create_publisher(vehVarName("simul_map_metadata", *veh), qosLatched); + // pub: /odom pubsubs.pub_odom = n_->create_publisher(vehVarName("odom", *veh), publisher_history_len_); @@ -613,7 +605,7 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) pubsubs.pub_collision = n_->create_publisher(vehVarName("collision", *veh), publisher_history_len_); - // "/tf", "/tf_static" + // pub: /tf, /tf_static rclcpp::QoS qosLatched10(rclcpp::KeepLast(10)); qosLatched10.durability(rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); @@ -649,7 +641,7 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) chassis_shape_msg.action = Msg_Marker::MODIFY; chassis_shape_msg.type = Msg_Marker::LINE_STRIP; - chassis_shape_msg.header.frame_id = vehVarName("base_link", *veh); + chassis_shape_msg.header.frame_id = "base_link"; chassis_shape_msg.ns = "mvsim.chassis_shape"; chassis_shape_msg.id = veh->getVehicleIndex(); chassis_shape_msg.scale.x = 0.05; @@ -756,10 +748,6 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) #endif } - // STATIC Identity transform /base_link -> /base_footprint - sendStaticTF( - vehVarName("base_link", *veh), vehVarName("base_footprint", *veh), tfIdentity_, myNow()); - // TF STATIC(namespace ): /base_link -> /base_footprint Msg_TransformStamped tx; tx.header.frame_id = "base_link"; @@ -813,8 +801,13 @@ void MVSimNode::spinNotifyROS() #if PACKAGE_ROS_VERSION == 2 // In ROS2,latching doesn't work, we must re-publish on a regular basis... - mvsim_world_->runVisitorOnWorldElements([this](mvsim::WorldElementBase& obj) - { publishWorldElements(obj); }); + for (size_t idx = 0; idx < vehs.size(); ++idx) + { + auto& pubs = pubsub_vehicles_[idx]; + + mvsim_world_->runVisitorOnWorldElements([this, &pubs](mvsim::WorldElementBase& obj) + { publishWorldElements(obj, pubs); }); + } #endif // Publish all TFs for each vehicle: @@ -831,9 +824,6 @@ void MVSimNode::spinNotifyROS() const auto& veh = it->second; auto& pubs = pubsub_vehicles_[i]; - const std::string sOdomName = vehVarName("odom", *veh); - const std::string sBaseLinkFrame = vehVarName("base_link", *veh); - // 1) Ground-truth pose and velocity // -------------------------------------------- const mrpt::math::TPose3D& gh_veh_pose = veh->getPose(); @@ -850,8 +840,8 @@ void MVSimNode::spinNotifyROS() gtOdoMsg.twist.twist.angular.z = gh_veh_vel.omega; gtOdoMsg.header.stamp = myNow(); - gtOdoMsg.header.frame_id = sOdomName; - gtOdoMsg.child_frame_id = sBaseLinkFrame; + gtOdoMsg.header.frame_id = "odom"; + gtOdoMsg.child_frame_id = "base_link"; pubs.pub_ground_truth->publish(gtOdoMsg); if (do_fake_localization_) @@ -875,17 +865,14 @@ void MVSimNode::spinNotifyROS() pubs.pub_amcl_pose->publish(currentPos); } - // TF: /map -> /odom + // TF(namespace ): /map -> /odom { Msg_TransformStamped tx; tx.header.frame_id = "map"; - tx.child_frame_id = sOdomName; + tx.child_frame_id = "odom"; tx.header.stamp = myNow(); tx.transform = tf2::toMsg(tf2::Transform::getIdentity()); - tf_br_.sendTransform(tx); - // TF(namespace ): /map -> /odom - tx.child_frame_id = "odom"; Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tx); pubs.pub_tf->publish(tfMsg); @@ -923,17 +910,14 @@ void MVSimNode::spinNotifyROS() { const mrpt::math::TPose3D odo_pose = gh_veh_pose; + // TF(namespace ): /odom -> /base_link { Msg_TransformStamped tx; - tx.header.frame_id = sOdomName; - tx.child_frame_id = sBaseLinkFrame; + tx.header.frame_id = "odom"; + tx.child_frame_id = "base_link"; tx.header.stamp = myNow(); tx.transform = tf2::toMsg(mrpt2ros::toROS_tfTransform(odo_pose)); - tf_br_.sendTransform(tx); - // TF(namespace ): /odom -> /base_link - tx.header.frame_id = "odom"; - tx.child_frame_id = "base_link"; Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tx); pubs.pub_tf->publish(tfMsg); @@ -946,8 +930,8 @@ void MVSimNode::spinNotifyROS() // first, we'll populate the header for the odometry msg odoMsg.header.stamp = myNow(); - odoMsg.header.frame_id = sOdomName; - odoMsg.child_frame_id = sBaseLinkFrame; + odoMsg.header.frame_id = "odom"; + odoMsg.child_frame_id = "base_link"; // publish: pubs.pub_odom->publish(odoMsg); @@ -1060,21 +1044,16 @@ void MVSimNode::internalOn( } lck.unlock(); - const std::string sSensorFrameId = vehVarName(obs.sensorLabel, veh); - // Send TF: mrpt::poses::CPose3D sensorPose = obs.sensorPose; auto transform = mrpt2ros::toROS_tfTransform(sensorPose); Msg_TransformStamped tfStmp; tfStmp.transform = tf2::toMsg(transform); - tfStmp.header.frame_id = vehVarName("base_link", veh); - tfStmp.child_frame_id = sSensorFrameId; - tfStmp.header.stamp = myNow(); - tf_br_.sendTransform(tfStmp); - tfStmp.header.frame_id = "base_link"; tfStmp.child_frame_id = obs.sensorLabel; + tfStmp.header.stamp = myNow(); + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); pubs.pub_tf->publish(tfMsg); @@ -1086,7 +1065,7 @@ void MVSimNode::internalOn( Msg_LaserScan msg_laser; // Force usage of simulation time: msg_laser.header.stamp = myNow(); - msg_laser.header.frame_id = sSensorFrameId; + msg_laser.header.frame_id = obs.sensorLabel; mrpt2ros::toROS(obs, msg_laser, msg_pose_laser); pub->publish(mvsim_node::make_shared(msg_laser)); } @@ -1113,21 +1092,16 @@ void MVSimNode::internalOn(const mvsim::VehicleBase& veh, const mrpt::obs::CObse } lck.unlock(); - const std::string sSensorFrameId = vehVarName(obs.sensorLabel, veh); - // Send TF: mrpt::poses::CPose3D sensorPose = obs.sensorPose; auto transform = mrpt2ros::toROS_tfTransform(sensorPose); Msg_TransformStamped tfStmp; tfStmp.transform = tf2::toMsg(transform); - tfStmp.header.frame_id = vehVarName("base_link", veh); - tfStmp.child_frame_id = sSensorFrameId; - tfStmp.header.stamp = myNow(); - tf_br_.sendTransform(tfStmp); - tfStmp.header.frame_id = "base_link"; tfStmp.child_frame_id = obs.sensorLabel; + tfStmp.header.stamp = myNow(); + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); pubs.pub_tf->publish(tfMsg); @@ -1140,7 +1114,7 @@ void MVSimNode::internalOn(const mvsim::VehicleBase& veh, const mrpt::obs::CObse Msg_Header msg_header; // Force usage of simulation time: msg_header.stamp = myNow(); - msg_header.frame_id = sSensorFrameId; + msg_header.frame_id = obs.sensorLabel; mrpt2ros::toROS(obs, msg_header, msg_imu); pub->publish(mvsim_node::make_shared(msg_imu)); } @@ -1167,8 +1141,6 @@ void MVSimNode::internalOn(const mvsim::VehicleBase& veh, const mrpt::obs::CObse } lck.unlock(); - const std::string sSensorFrameId = vehVarName(obs.sensorLabel, veh); - // Send TF: mrpt::poses::CPose3D sensorPose; obs.getSensorPose(sensorPose); @@ -1176,13 +1148,10 @@ void MVSimNode::internalOn(const mvsim::VehicleBase& veh, const mrpt::obs::CObse Msg_TransformStamped tfStmp; tfStmp.transform = tf2::toMsg(transform); - tfStmp.header.frame_id = vehVarName("base_link", veh); - tfStmp.child_frame_id = sSensorFrameId; - tfStmp.header.stamp = myNow(); - tf_br_.sendTransform(tfStmp); - tfStmp.header.frame_id = "base_link"; tfStmp.child_frame_id = obs.sensorLabel; + tfStmp.header.stamp = myNow(); + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); pubs.pub_tf->publish(tfMsg); @@ -1193,7 +1162,7 @@ void MVSimNode::internalOn(const mvsim::VehicleBase& veh, const mrpt::obs::CObse Msg_Image msg_img; Msg_Header msg_header; msg_header.stamp = myNow(); - msg_header.frame_id = sSensorFrameId; + msg_header.frame_id = obs.sensorLabel; msg_img = mrpt2ros::toROS(obs.image, msg_header); pub->publish(mvsim_node::make_shared(msg_img)); } @@ -1232,9 +1201,6 @@ void MVSimNode::internalOn( } lck.unlock(); - const std::string sSensorFrameId_image = vehVarName(lbImage, veh); - const std::string sSensorFrameId_points = vehVarName(lbPoints, veh); - const auto now = myNow(); // IMAGE @@ -1247,13 +1213,10 @@ void MVSimNode::internalOn( Msg_TransformStamped tfStmp; tfStmp.transform = tf2::toMsg(transform); - tfStmp.header.frame_id = vehVarName("base_link", veh); - tfStmp.child_frame_id = sSensorFrameId_image; - tfStmp.header.stamp = now; - tf_br_.sendTransform(tfStmp); - tfStmp.header.frame_id = "base_link"; tfStmp.child_frame_id = lbImage; + tfStmp.header.stamp = now; + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); pubs.pub_tf->publish(tfMsg); @@ -1264,7 +1227,7 @@ void MVSimNode::internalOn( Msg_Image msg_img; Msg_Header msg_header; msg_header.stamp = now; - msg_header.frame_id = sSensorFrameId_image; + msg_header.frame_id = lbImage; msg_img = mrpt2ros::toROS(obs.intensityImage, msg_header); pubImg->publish(mvsim_node::make_shared(msg_img)); } @@ -1280,13 +1243,10 @@ void MVSimNode::internalOn( Msg_TransformStamped tfStmp; tfStmp.transform = tf2::toMsg(transform); - tfStmp.header.frame_id = vehVarName("base_link", veh); - tfStmp.child_frame_id = sSensorFrameId_points; - tfStmp.header.stamp = now; - tf_br_.sendTransform(tfStmp); - tfStmp.header.frame_id = "base_link"; tfStmp.child_frame_id = lbPoints; + tfStmp.header.stamp = now; + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); pubs.pub_tf->publish(tfMsg); @@ -1297,7 +1257,7 @@ void MVSimNode::internalOn( Msg_PointCloud2 msg_pts; Msg_Header msg_header; msg_header.stamp = now; - msg_header.frame_id = sSensorFrameId_points; + msg_header.frame_id = lbPoints; mrpt::obs::T3DPointsProjectionParams pp; pp.takeIntoAccountSensorPoseOnRobot = false; @@ -1337,8 +1297,6 @@ void MVSimNode::internalOn( } lck.unlock(); - const std::string sSensorFrameId_points = vehVarName(lbPoints, veh); - const auto now = myNow(); // POINTS @@ -1350,13 +1308,10 @@ void MVSimNode::internalOn( Msg_TransformStamped tfStmp; tfStmp.transform = tf2::toMsg(transform); - tfStmp.header.frame_id = vehVarName("base_link", veh); - tfStmp.child_frame_id = sSensorFrameId_points; - tfStmp.header.stamp = now; - tf_br_.sendTransform(tfStmp); - tfStmp.header.frame_id = "base_link"; tfStmp.child_frame_id = lbPoints; + tfStmp.header.stamp = now; + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); pubs.pub_tf->publish(tfMsg); @@ -1367,7 +1322,7 @@ void MVSimNode::internalOn( Msg_PointCloud2 msg_pts; Msg_Header msg_header; msg_header.stamp = now; - msg_header.frame_id = sSensorFrameId_points; + msg_header.frame_id = lbPoints; #if defined(HAVE_POINTS_XYZIRT) if (auto* xyzirt = dynamic_cast(obs.pointcloud.get()); diff --git a/mvsim_tutorial/cpp/mvsim-subscriber-example/mvsim-subscriber-example.cpp b/mvsim_tutorial/cpp/mvsim-subscriber-example/mvsim-subscriber-example.cpp index e2118554..e928588a 100644 --- a/mvsim_tutorial/cpp/mvsim-subscriber-example/mvsim-subscriber-example.cpp +++ b/mvsim_tutorial/cpp/mvsim-subscriber-example/mvsim-subscriber-example.cpp @@ -16,14 +16,12 @@ void myPoseCallback(const mvsim_msgs::TimeStampedPose& p) void mySensorCallback(const mvsim_msgs::GenericObservation& o) { const std::vector data( - o.mrptserializedobservation().begin(), - o.mrptserializedobservation().end()); + o.mrptserializedobservation().begin(), o.mrptserializedobservation().end()); mrpt::serialization::CSerializable::Ptr obj; mrpt::serialization::OctetVectorToObject(data, obj); - mrpt::obs::CObservation::Ptr obs = - std::dynamic_pointer_cast(obj); + mrpt::obs::CObservation::Ptr obs = std::dynamic_pointer_cast(obj); ASSERT_(obs); std::cout << "sensor callback: " << obs->asString() << "\n"; @@ -38,11 +36,9 @@ int main(int argc, char** argv) client.connect(); - client.subscribeTopic( - "/r1/pose", myPoseCallback); + client.subscribeTopic("/r1/pose", myPoseCallback); - client.subscribeTopic( - "/r1/laser1", mySensorCallback); + client.subscribeTopic("/r1/laser1", mySensorCallback); std::this_thread::sleep_for(std::chrono::seconds(5)); diff --git a/mvsim_tutorial/demo_1robot_ros2.rviz b/mvsim_tutorial/demo_1robot_ros2.rviz index ed08651c..ce13063b 100644 --- a/mvsim_tutorial/demo_1robot_ros2.rviz +++ b/mvsim_tutorial/demo_1robot_ros2.rviz @@ -23,6 +23,9 @@ Panels: Splitter Ratio: 0.5 - Class: rviz_common/Transformation Name: Transformation + - Class: rviz_plugin_tutorials/Teleop + Name: Teleop + Topic: /cmd_vel Visualization Manager: Class: "" Displays: @@ -275,6 +278,8 @@ Window Geometry: QMainWindow State: 000000ff00000000fd0000000400000000000001560000037dfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000037d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001c005400720061006e00730066006f0072006d006100740069006f006e00000002fb000000bf0000007b00ffffff000000010000010f0000037dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000037d000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005dc0000037d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false + Teleop: + collapsed: false Tool Properties: collapsed: false Transformation: diff --git a/mvsim_tutorial/demo_2robots.launch b/mvsim_tutorial/demo_2robots.launch index 7e1b34bb..489342f4 100644 --- a/mvsim_tutorial/demo_2robots.launch +++ b/mvsim_tutorial/demo_2robots.launch @@ -4,9 +4,30 @@ - + - + + + + + + + + + + + + + + + + + + + + + + diff --git a/mvsim_tutorial/demo_2robots.launch.py b/mvsim_tutorial/demo_2robots.launch.py index f6820c79..23ee49b4 100644 --- a/mvsim_tutorial/demo_2robots.launch.py +++ b/mvsim_tutorial/demo_2robots.launch.py @@ -34,17 +34,42 @@ def generate_launch_description(): }] ) - rviz2_node = Node( + rviz2_r1_node = Node( package='rviz2', executable='rviz2', name='rviz2', + namespace='r1', arguments=[ - '-d', [os.path.join(mvsimDir, 'mvsim_tutorial', 'demo_2robots_ros2.rviz')]] + '-d', [os.path.join(mvsimDir, 'mvsim_tutorial', 'demo_2robots_r1_ros2.rviz')]], + output='screen', + remappings=[('/map', 'map'), + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ('/goal_pose', 'goal_pose'), + ('/clicked_point', 'clicked_point'), + ('/initialpose', 'initialpose')] + ) + + rviz2_r2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + namespace='r2', + arguments=[ + '-d', [os.path.join(mvsimDir, 'mvsim_tutorial', 'demo_2robots_r2_ros2.rviz')]], + output='screen', + remappings=[('/map', 'map'), + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ('/goal_pose', 'goal_pose'), + ('/clicked_point', 'clicked_point'), + ('/initialpose', 'initialpose')] ) return LaunchDescription([ world_file_launch_arg, do_fake_localization_arg, mvsim_node, - rviz2_node + rviz2_r1_node, + rviz2_r2_node ]) diff --git a/mvsim_tutorial/demo_2robots.rviz b/mvsim_tutorial/demo_2robots_r1.rviz similarity index 73% rename from mvsim_tutorial/demo_2robots.rviz rename to mvsim_tutorial/demo_2robots_r1.rviz index a57dbc00..592bcf57 100644 --- a/mvsim_tutorial/demo_2robots.rviz +++ b/mvsim_tutorial/demo_2robots_r1.rviz @@ -30,9 +30,6 @@ Panels: - Class: rviz_plugin_tutorials/Teleop Name: Teleop Topic: /r1/cmd_vel - - Class: rviz_plugin_tutorials/Teleop - Name: Teleop - Topic: /r2/cmd_vel Visualization Manager: Class: "" Displays: @@ -54,59 +51,39 @@ Visualization Manager: Plane Cell Count: 80 Reference Frame: Value: true - - Alpha: 0.7 - Class: rviz/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Map - Topic: /simul_map - Value: true - Class: rviz/TF Enabled: true Frame Timeout: 15 Frames: All Enabled: false - map: - Value: true - r1/base_footprint: - Value: false - r1/base_link: + base_link: Value: true - r1/laser1: + camera1_image: Value: true - r1/odom: + camera1_points: Value: true - r2/base_footprint: - Value: false - r2/base_link: + laser1: Value: true - r2/laser1: - Value: true - r2/odom: + map: Value: true - world: + odom: Value: true + Marker Alpha: 1 Marker Scale: 4 Name: TF Show Arrows: false Show Axes: true Show Names: true Tree: - world: - map: - r1/odom: - r1/base_link: - r1/base_footprint: - {} - r1/laser1: - {} - r2/odom: - r2/base_link: - r2/base_footprint: - {} - r2/laser1: - {} + map: + odom: + base_link: + camera1_image: + {} + camera1_points: + {} + laser1: + {} Update Interval: 0 Value: true - Alpha: 1 @@ -138,35 +115,6 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: z - Class: rviz/LaserScan - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 0; 136; 255 - Max Intensity: 0 - Min Color: 0; 136; 255 - Min Intensity: 0 - Name: LaserScan - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.1 - Style: Flat Squares - Topic: /r2/laser1 - Use Fixed Frame: true - Use rainbow: false - Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /r1/chassis_markers @@ -177,18 +125,6 @@ Visualization Manager: mvsim.chassis_shape.wheel1: true Queue Size: 100 Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /r2/chassis_markers - Name: MarkerArray - Namespaces: - mvsim.chassis_shape: true - mvsim.chassis_shape.wheel0: true - mvsim.chassis_shape.wheel1: true - mvsim.chassis_shape.wheel2: true - mvsim.chassis_shape.wheel3: true - Queue Size: 100 - Value: true Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/mvsim_tutorial/demo_2robots_ros2.rviz b/mvsim_tutorial/demo_2robots_r1_ros2.rviz similarity index 64% rename from mvsim_tutorial/demo_2robots_ros2.rviz rename to mvsim_tutorial/demo_2robots_r1_ros2.rviz index 731019e7..28026d77 100644 --- a/mvsim_tutorial/demo_2robots_ros2.rviz +++ b/mvsim_tutorial/demo_2robots_r1_ros2.rviz @@ -1,13 +1,13 @@ Panels: - Class: rviz_common/Displays - Help Height: 78 + Help Height: 138 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 Splitter Ratio: 0.5 - Tree Height: 787 + Tree Height: 219 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -23,6 +23,9 @@ Panels: Splitter Ratio: 0.5 - Class: rviz_common/Transformation Name: Transformation + - Class: rviz_plugin_tutorials/Teleop + Name: Teleop + Topic: /r1/cmd_vel Visualization Manager: Class: "" Displays: @@ -49,31 +52,19 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: true - map: - Value: true - r1/base_footprint: - Value: true - r1/base_link: - Value: true - r1/camera1_image: + base_footprint: Value: true - r1/camera1_points: + base_link: Value: true - r1/laser1: + camera1_image: Value: true - r1/odom: + camera1_points: Value: true - r2/base_footprint: + laser1: Value: true - r2/base_link: - Value: true - r2/camera1: - Value: true - r2/laser1: - Value: true - r2/odom: + map: Value: true - world: + odom: Value: true Marker Scale: 1 Name: TF @@ -81,49 +72,19 @@ Visualization Manager: Show Axes: true Show Names: false Tree: - world: - map: - r1/odom: - r1/base_link: - r1/base_footprint: - {} - r1/camera1_image: - {} - r1/camera1_points: - {} - r1/laser1: - {} - r2/odom: - r2/base_link: - r2/base_footprint: - {} - r2/camera1: - {} - r2/laser1: - {} + map: + odom: + base_link: + base_footprint: + {} + camera1_image: + {} + camera1_points: + {} + laser1: + {} Update Interval: 0 Value: true - - Alpha: 0.699999988079071 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Map - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /simul_map - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /simul_map_updates - Use Timestamp: false - Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -158,40 +119,6 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/LaserScan - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: LaserScan - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /r2/laser1 - Use Fixed Frame: true - Use rainbow: true - Value: true - Alpha: 1 Arrow Length: 0.30000001192092896 Axes Length: 0.30000001192092896 @@ -217,13 +144,29 @@ Visualization Manager: Enabled: true Name: MarkerArray Namespaces: - {} + mvsim.chassis_shape: true + mvsim.chassis_shape.wheel0: true + mvsim.chassis_shape.wheel1: true Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /chassis_markers + Value: /r1/chassis_markers + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /r1/camera1_image Value: true Enabled: true Global Options: @@ -278,7 +221,7 @@ Visualization Manager: Swap Stereo Eyes: false Value: false Focal Point: - X: 14.356273651123047 + X: 7.178136825561523 Y: -33.14567184448242 Z: -26.374494552612305 Focal Shape Fixed Size: true @@ -294,18 +237,22 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 1016 + Height: 960 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001c005400720061006e00730066006f0072006d006100740069006f006e00000002fb000000bf0000007b00ffffff000000010000010f0000037dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000037d000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005da0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001f70000035cfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000001d30000018200fffffffb0000000a0049006d006100670065010000024d0000009f0000004a00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001c005400720061006e00730066006f0072006d006100740069006f006e00000002fb000000bf000000ec00fffffffb0000000c00540065006c0065006f007001000002f8000000d20000008000ffffff000000010000010f0000037dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000037d0000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005330000035c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false + Teleop: + collapsed: false Tool Properties: collapsed: false Transformation: collapsed: false Views: collapsed: false - Width: 1846 - X: 1154 - Y: 27 + Width: 1440 + X: 400 + Y: 100 diff --git a/mvsim_tutorial/demo_2robots_r2.rviz b/mvsim_tutorial/demo_2robots_r2.rviz new file mode 100644 index 00000000..4ffea9c8 --- /dev/null +++ b/mvsim_tutorial/demo_2robots_r2.rviz @@ -0,0 +1,184 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.557312 + Tree Height: 362 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan + - Class: rviz_plugin_tutorials/Teleop + Name: Teleop + Topic: /r2/cmd_vel +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 80 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_link: + Value: true + camera1: + Value: true + laser1: + Value: true + map: + Value: true + odom: + Value: true + Marker Alpha: 1 + Marker Scale: 4 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: true + Tree: + map: + odom: + base_link: + camera1: + {} + laser1: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: z + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.1 + Style: Flat Squares + Topic: /r2/laser1 + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /r2/chassis_markers + Name: MarkerArray + Namespaces: + mvsim.chassis_shape: true + mvsim.chassis_shape.wheel0: true + mvsim.chassis_shape.wheel1: true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 24.1546 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -4.57125 + Y: 2.0506 + Z: -0.341872 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.874797 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.5154 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 876 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000010f000002e2fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001ab000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00540065006c0065006f007001000001d9000000a00000004900fffffffb0000000c00540065006c0065006f0070010000027f0000008b0000004900ffffff000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff0000003efc0100000002fb0000000800540069006d00650100000000000005ff000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004ea000002e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Teleop: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1535 + X: 1089 + Y: 24 diff --git a/mvsim_tutorial/demo_2robots_r2_ros2.rviz b/mvsim_tutorial/demo_2robots_r2_ros2.rviz new file mode 100644 index 00000000..b4347e1c --- /dev/null +++ b/mvsim_tutorial/demo_2robots_r2_ros2.rviz @@ -0,0 +1,256 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 219 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Transformation + Name: Transformation + - Class: rviz_plugin_tutorials/Teleop + Name: Teleop + Topic: /r2/cmd_vel +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_footprint: + Value: true + base_link: + Value: true + camera1: + Value: true + laser1: + Value: true + map: + Value: true + odom: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + map: + odom: + base_link: + base_footprint: + {} + camera1: + {} + laser1: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /r2/laser1 + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: PoseArray + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /r2/particlecloud + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + mvsim.chassis_shape: true + mvsim.chassis_shape.wheel0: true + mvsim.chassis_shape.wheel1: true + mvsim.chassis_shape.wheel2: true + mvsim.chassis_shape.wheel3: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /r2/chassis_markers + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /r2/camera1 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 71.78666687011719 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 7.178136825561523 + Y: -33.14567184448242 + Z: -26.374494552612305 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6353979110717773 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.730397343635559 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 960 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001f70000035cfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000001d30000018200fffffffb0000000a0049006d006100670065010000024d0000009f0000004a00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001c005400720061006e00730066006f0072006d006100740069006f006e00000002fb000000bf000000ec00fffffffb0000000c00540065006c0065006f007001000002f8000000d20000008000ffffff000000010000010f0000037dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000037d0000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005330000035c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Teleop: + collapsed: false + Tool Properties: + collapsed: false + Transformation: + collapsed: false + Views: + collapsed: false + Width: 1440 + X: 800 + Y: 200 diff --git a/mvsim_tutorial/demo_depth_camera_ros2.rviz b/mvsim_tutorial/demo_depth_camera_ros2.rviz index bc4854d6..82c1a065 100644 --- a/mvsim_tutorial/demo_depth_camera_ros2.rviz +++ b/mvsim_tutorial/demo_depth_camera_ros2.rviz @@ -24,6 +24,9 @@ Panels: Splitter Ratio: 0.5 - Class: rviz_common/Transformation Name: Transformation + - Class: rviz_plugin_tutorials/Teleop + Name: Teleop + Topic: /cmd_vel Visualization Manager: Class: "" Displays: @@ -307,6 +310,8 @@ Window Geometry: QMainWindow State: 000000ff00000000fd0000000400000000000002040000039efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001cd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001c005400720061006e00730066006f0072006d006100740069006f006e00000002fb000000bf0000007b00fffffffb0000000a0049006d0061006700650100000210000001cb0000002800ffffff000000010000010f0000037dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000037d000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000052e0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false + Teleop: + collapsed: false Tool Properties: collapsed: false Transformation: diff --git a/mvsim_tutorial/demo_greenhouse_ros2.rviz b/mvsim_tutorial/demo_greenhouse_ros2.rviz index 51a25a89..e47d8bca 100644 --- a/mvsim_tutorial/demo_greenhouse_ros2.rviz +++ b/mvsim_tutorial/demo_greenhouse_ros2.rviz @@ -23,6 +23,9 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 + - Class: rviz_plugin_tutorials/Teleop + Name: Teleop + Topic: /cmd_vel Visualization Manager: Class: "" Displays: @@ -260,6 +263,8 @@ Window Geometry: QMainWindow State: 000000ff00000000fd00000004000000000000016b0000039efc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065010000003d000001330000002800fffffffb000000100044006900730070006c006100790073010000017600000265000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005c90000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false + Teleop: + collapsed: false Tool Properties: collapsed: false Views: diff --git a/mvsim_tutorial/demo_turtlebot_world.launch.py b/mvsim_tutorial/demo_turtlebot_world.launch.py index e0ed4694..4070166d 100644 --- a/mvsim_tutorial/demo_turtlebot_world.launch.py +++ b/mvsim_tutorial/demo_turtlebot_world.launch.py @@ -36,7 +36,7 @@ def generate_launch_description(): MVSIM_ROS2_PARAMS_FILE, { "world_file": LaunchConfiguration('world_file'), - "headless": False + "headless": False, "do_fake_localization": LaunchConfiguration('do_fake_localization'), }] ) diff --git a/mvsim_tutorial/demo_turtlebot_world_ros2.rviz b/mvsim_tutorial/demo_turtlebot_world_ros2.rviz index d3281989..55c2277a 100644 --- a/mvsim_tutorial/demo_turtlebot_world_ros2.rviz +++ b/mvsim_tutorial/demo_turtlebot_world_ros2.rviz @@ -24,6 +24,9 @@ Panels: Splitter Ratio: 0.5 - Class: rviz_common/Transformation Name: Transformation + - Class: rviz_plugin_tutorials/Teleop + Name: Teleop + Topic: /cmd_vel Visualization Manager: Class: "" Displays: @@ -276,6 +279,8 @@ Window Geometry: QMainWindow State: 000000ff00000000fd0000000400000000000002040000039efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001cd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001c005400720061006e00730066006f0072006d006100740069006f006e00000002fb000000bf0000007b00fffffffb0000000a0049006d0061006700650100000210000001cb0000002800ffffff000000010000010f0000037dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000037d000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000052c0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false + Teleop: + collapsed: false Tool Properties: collapsed: false Transformation: diff --git a/mvsim_tutorial/demo_warehouse_ros2.rviz b/mvsim_tutorial/demo_warehouse_ros2.rviz index 51a25a89..e47d8bca 100644 --- a/mvsim_tutorial/demo_warehouse_ros2.rviz +++ b/mvsim_tutorial/demo_warehouse_ros2.rviz @@ -23,6 +23,9 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 + - Class: rviz_plugin_tutorials/Teleop + Name: Teleop + Topic: /cmd_vel Visualization Manager: Class: "" Displays: @@ -260,6 +263,8 @@ Window Geometry: QMainWindow State: 000000ff00000000fd00000004000000000000016b0000039efc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065010000003d000001330000002800fffffffb000000100044006900730070006c006100790073010000017600000265000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005c90000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false + Teleop: + collapsed: false Tool Properties: collapsed: false Views: diff --git a/tests/test_shape2p5.cpp b/tests/test_shape2p5.cpp index aeda954a..f64154e4 100644 --- a/tests/test_shape2p5.cpp +++ b/tests/test_shape2p5.cpp @@ -52,11 +52,11 @@ void shape_test_merge() const float radius = 0.5f, L = 2.0f; auto glCyl = mrpt::opengl::CCylinder::Create(radius, radius, L); - const Shape2p5 s1 = csc.get( - *glCyl, 0, L, mrpt::poses::CPose3D::FromTranslation(-0.15, 0, 0), 1.0f); + const Shape2p5 s1 = + csc.get(*glCyl, 0, L, mrpt::poses::CPose3D::FromTranslation(-0.15, 0, 0), 1.0f); - const Shape2p5 s2 = csc.get( - *glCyl, 0, L, mrpt::poses::CPose3D::FromTranslation(0.15, 0, 0), 1.0f); + const Shape2p5 s2 = + csc.get(*glCyl, 0, L, mrpt::poses::CPose3D::FromTranslation(0.15, 0, 0), 1.0f); Shape2p5 s = s1; s.mergeWith(s2); @@ -71,8 +71,7 @@ void shape_test_simplecamera() auto& csc = CollisionShapeCache::Instance(); auto glModel = mrpt::opengl::CAssimpModel::Create(); - glModel->loadScene(mrpt::system::pathJoin( - {MVSIM_TEST_DIR, "../models/simple_camera.dae"})); + glModel->loadScene(mrpt::system::pathJoin({MVSIM_TEST_DIR, "../models/simple_camera.dae"})); const Shape2p5 shape = csc.get(*glModel, 0, 1.0, {}, 1.0f); diff --git a/tests/test_utils.h b/tests/test_utils.h index 34531756..a11ad438 100644 --- a/tests/test_utils.h +++ b/tests/test_utils.h @@ -9,23 +9,20 @@ #pragma once -#include #include +#include static inline void setConsoleErrorColor() { - mrpt::system::consoleColorAndStyle( - mrpt::system::ConsoleForegroundColor::RED); + mrpt::system::consoleColorAndStyle(mrpt::system::ConsoleForegroundColor::RED); } static inline void setConsoleNormalColor() { - mrpt::system::consoleColorAndStyle( - mrpt::system::ConsoleForegroundColor::DEFAULT); + mrpt::system::consoleColorAndStyle(mrpt::system::ConsoleForegroundColor::DEFAULT); } static inline void setConsoleBlueColor() { - mrpt::system::consoleColorAndStyle( - mrpt::system::ConsoleForegroundColor::BLUE); + mrpt::system::consoleColorAndStyle(mrpt::system::ConsoleForegroundColor::BLUE); }