Skip to content

Commit

Permalink
Fix build without ZMQ
Browse files Browse the repository at this point in the history
  • Loading branch information
jlblancoc committed Dec 6, 2024
1 parent 32f460e commit 46974eb
Show file tree
Hide file tree
Showing 12 changed files with 87 additions and 18 deletions.
12 changes: 10 additions & 2 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -346,13 +346,21 @@ if (BUILD_FOR_ROS)
PACKAGE_ROS_VERSION=${PACKAGE_ROS_VERSION}
)

if (TARGET mvsim::comms)
set(DEP_MVSIM_COMMS_TRG mvsim::comms)
endif()
if (TARGET mvsim::msgs)
set(DEP_MVSIM_MSGS_TRG mvsim::msgs)
endif()


# Specify libraries to link a library or executable target against
target_link_libraries(
mvsim_node
#PRIVATE # ros ament already used the plain signature, we cannot use this here...
mvsim::simulator
mvsim::msgs
mvsim::comms
${DEP_MVSIM_MSGS_TRG}
${DEP_MVSIM_COMMS_TRG}
${BOX2D_LIBRARIES} # Box2D libs
${catkin_LIBRARIES}
${CMAKE_THREAD_LIBS_INIT}
Expand Down
20 changes: 12 additions & 8 deletions modules/simulator/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -119,29 +119,34 @@ source_group(Friction FILES ${Friction_SRCS})
source_group(VehicleDynamics FILES ${VehicleDynamics_SRCS})
source_group(WorldElements FILES ${WorldElements_SRCS})

if (TARGET mvsim::comms)
set(DEP_MVSIM_COMMS_PKG mvsim-comms)
set(DEP_MVSIM_COMMS_TRG mvsim::comms)
endif()
if (TARGET mvsim::msgs)
set(DEP_MVSIM_MSGS_TRG mvsim::msgs)
endif()

set(PACKAGE_DEPENDENCIES
mrpt-maps
mrpt-gui
mrpt-topography
mvsim-comms
${DEP_MVSIM_COMMS_PKG}
box2d
)
mvsim_common_target_settings(${MODULE_NAME})


target_link_libraries(${PROJECT_NAME}
PRIVATE
mvsim::msgs
# ${CMAKE_THREAD_LIBS_INIT}
# ${Protobuf_LIBRARIES}
# ${ZeroMQ_LIBRARY}
PRIVATE
${DEP_MVSIM_MSGS_TRG}
PUBLIC
mrpt::obs
mrpt::opengl
mrpt::maps
mrpt::gui
mrpt::topography
mvsim::comms
${DEP_MVSIM_COMMS_TRG}
${BOX2D_LIBRARIES}
)

Expand All @@ -150,4 +155,3 @@ target_include_directories(${PROJECT_NAME}
SYSTEM PUBLIC
$<BUILD_INTERFACE:${mvsim_SOURCE_DIR}/externals/rapidxml>
)

9 changes: 8 additions & 1 deletion modules/simulator/include/mvsim/World.h
Original file line number Diff line number Diff line change
Expand Up @@ -26,13 +26,16 @@
#include <mrpt/system/CTimeLogger.h>
#include <mrpt/topography/data_types.h>
#include <mvsim/Block.h>
#include <mvsim/Comms/Client.h>
#include <mvsim/Joystick.h>
#include <mvsim/RemoteResourcesManager.h>
#include <mvsim/TParameterDefinitions.h>
#include <mvsim/VehicleBase.h>
#include <mvsim/WorldElements/WorldElementBase.h>

#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
#include <mvsim/Comms/Client.h>
#endif

#include <functional>
#include <list>
#include <map>
Expand Down Expand Up @@ -370,8 +373,10 @@ class World : public mrpt::system::COutputLogger
* description loaded from XML file. */
void connectToServer();

#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
mvsim::Client& commsClient() { return client_; }
const mvsim::Client& commsClient() const { return client_; }
#endif

void free_opengl_resources();

Expand Down Expand Up @@ -414,7 +419,9 @@ class World : public mrpt::system::COutputLogger
friend class VehicleBase;
friend class Block;

#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
mvsim::Client client_{"World"};
#endif

std::vector<on_observation_callback_t> callbacksOnObservation_;

Expand Down
7 changes: 6 additions & 1 deletion modules/simulator/src/Simulable.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -10,12 +10,15 @@
#include <box2d/b2_contact.h>
#include <box2d/b2_distance.h>
#include <mvsim/Block.h>
#include <mvsim/Comms/Client.h>
#include <mvsim/Simulable.h>
#include <mvsim/TParameterDefinitions.h>
#include <mvsim/VisualObject.h>
#include <mvsim/World.h>

#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
#include <mvsim/Comms/Client.h>
#endif

#include <cmath> // fmod()

#include "JointXMLnode.h"
Expand Down Expand Up @@ -378,6 +381,7 @@ bool Simulable::parseSimulable(const JointXMLnode<>& rootNode, const ParseSimula

void Simulable::internalHandlePublish(const TSimulContext& context)
{
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
std::shared_lock lck(q_mtx_);

MRPT_START
Expand Down Expand Up @@ -448,6 +452,7 @@ void Simulable::internalHandlePublish(const TSimulContext& context)
}

MRPT_END
#endif
}

void Simulable::registerOnServer(mvsim::Client& c)
Expand Down
6 changes: 3 additions & 3 deletions modules/simulator/src/World.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -10,14 +10,12 @@
#include <mrpt/math/TTwist2D.h>
#include <mrpt/obs/CObservationOdometry.h>
#include <mrpt/poses/CPose3DQuat.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/filesystem.h> // filePathSeparatorsToNative()
#include <mrpt/version.h>
#include <mvsim/World.h>

#include <algorithm> // count()
#include <iostream>
#include <map>
#include <stdexcept>

using namespace mvsim;
using namespace std;
Expand Down Expand Up @@ -144,6 +142,7 @@ void World::runVisitorOnBlocks(const block_visitor_t& v)

void World::connectToServer()
{
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
//
client_.setVerbosityLevel(this->getMinLoggingLevel());
client_.serverHostAddress(serverAddress_);
Expand All @@ -161,6 +160,7 @@ void World::connectToServer()

// global services:
internal_advertiseServices();
#endif
}

void World::insertBlock(const Block::Ptr& block)
Expand Down
12 changes: 10 additions & 2 deletions mvsim-cli/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,5 +1,13 @@
project(mvsim)

if (TARGET mvsim::comms)
set(DEP_MVSIM_COMMS_TRG mvsim::comms)
endif()
if (TARGET mvsim::msgs)
set(DEP_MVSIM_MSGS_TRG mvsim::msgs)
endif()


# === Define executable ===
add_executable(${PROJECT_NAME}
mvsim-cli-main.cpp
Expand All @@ -12,8 +20,8 @@ add_executable(${PROJECT_NAME}
target_link_libraries(
${PROJECT_NAME}
mvsim::simulator
mvsim::msgs
mvsim::comms
${DEP_MVSIM_MSGS_TRG}
${DEP_MVSIM_COMMS_TRG}
mrpt::tclap
${BOX2D_LIBRARIES} # Box2D libs
${CMAKE_THREAD_LIBS_INIT}
Expand Down
6 changes: 6 additions & 0 deletions mvsim-cli/mvsim-cli-node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,11 @@
| See COPYING |
+-------------------------------------------------------------------------+ */

#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
#include <mvsim/Comms/Client.h>
#endif

#include <map>

#include "mvsim-cli.h"

Expand Down Expand Up @@ -36,6 +40,7 @@ int commandNode()

int nodeList()
{
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
mvsim::Client client;

client.setMinLoggingLevel(mrpt::typemeta::TEnumType<mrpt::system::VerbosityLevel>::name2value(
Expand All @@ -53,6 +58,7 @@ int nodeList()
{
std::cout << "- name: \"" << n.name << "\"\n";
}
#endif

return 0;
}
Expand Down
9 changes: 9 additions & 0 deletions mvsim-cli/mvsim-cli-server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -8,15 +8,21 @@
+-------------------------------------------------------------------------+ */

#include <mrpt/core/exceptions.h>

#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
#include <mvsim/Comms/Server.h>
#include <mvsim/Comms/ports.h> // MVSIM_PORTNO_MAIN_REP
#endif

#include "mvsim-cli.h"

#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
std::shared_ptr<mvsim::Server> server;
#endif

void commonLaunchServer()
{
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
ASSERT_(!server);

// Start network server:
Expand All @@ -28,10 +34,12 @@ void commonLaunchServer()
cli->argVerbosity.getValue()));

server->start();
#endif
}

int launchStandAloneServer()
{
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
if (cli->argHelp.isSet())
{
fprintf(
Expand All @@ -45,6 +53,7 @@ Available options:
mvsim::MVSIM_PORTNO_MAIN_REP, mvsim::MVSIM_PORTNO_MAIN_REP);
return 0;
}
#endif

commonLaunchServer();
return 0;
Expand Down
11 changes: 11 additions & 0 deletions mvsim-cli/mvsim-cli-topic.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -11,9 +11,12 @@
#include <mrpt/math/distributions.h>
#include <mrpt/math/ops_containers.h>
#include <mrpt/system/datetime.h>

#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
#include <mvsim/Comms/Client.h>
#include <mvsim/mvsim-msgs/ObservationLidar2D.pb.h>
#include <mvsim/mvsim-msgs/TimeStampedPose.pb.h>
#endif

#include "mvsim-cli.h"

Expand Down Expand Up @@ -48,6 +51,7 @@ int commandTopic()

int topicList()
{
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
mvsim::Client client;

client.setMinLoggingLevel(mrpt::typemeta::TEnumType<mrpt::system::VerbosityLevel>::name2value(
Expand Down Expand Up @@ -86,9 +90,11 @@ int topicList()
std::cout << n.publishers.at(0) << "\n";
}
}
#endif
return 0;
}

#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
static void echo_TimeStampedPose(const std::string& data)
{
mvsim_msgs::TimeStampedPose out;
Expand Down Expand Up @@ -125,9 +131,11 @@ static void callbackSubscribeTopicGeneric(const zmq::message_t& msg)

std::cout << std::endl;
}
#endif

int topicEcho()
{
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
mvsim::Client client;

client.setMinLoggingLevel(mrpt::typemeta::TEnumType<mrpt::system::VerbosityLevel>::name2value(
Expand All @@ -150,11 +158,13 @@ int topicEcho()
{
}

#endif
return 0;
}

int topicHz()
{
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
mvsim::Client client;

client.setMinLoggingLevel(mrpt::typemeta::TEnumType<mrpt::system::VerbosityLevel>::name2value(
Expand Down Expand Up @@ -229,6 +239,7 @@ int topicHz()
std::cout << "- Period_95percent: " << tHigh << " # [sec]" << std::endl;
}

#endif
return 0;
}

Expand Down
4 changes: 4 additions & 0 deletions mvsim-cli/mvsim-cli.h
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,9 @@
#pragma once

#include <mrpt/3rdparty/tclap/CmdLine.h>
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
#include <mvsim/Comms/ports.h>
#endif

#include <functional>
#include <memory>
Expand Down Expand Up @@ -42,8 +44,10 @@ struct cli_flags

TCLAP::SwitchArg argHelp{"h", "help", "Shows more detailed help for command", cmd};

#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
TCLAP::ValueArg<int> argPort{
"p", "port", "TCP port to listen at", false, mvsim::MVSIM_PORTNO_MAIN_REP, "TCP port", cmd};
#endif

TCLAP::ValueArg<double> argRealTimeFactor{
"",
Expand Down
7 changes: 6 additions & 1 deletion mvsim_node_src/include/mvsim/mvsim_node_core.h
Original file line number Diff line number Diff line change
Expand Up @@ -17,10 +17,13 @@
#include <mrpt/obs/CObservationPointCloud.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/CTimeLogger.h>
#include <mvsim/Comms/Server.h>
#include <mvsim/World.h>
#include <tf2/LinearMath/Transform.h>

#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
#include <mvsim/Comms/Server.h>
#endif

#include <atomic>
#include <thread>

Expand Down Expand Up @@ -166,7 +169,9 @@ class MVSimNode
int publisher_history_len_ = 50;

protected:
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
mvsim_node::shared_ptr<mvsim::Server> mvsim_server_;
#endif

#if PACKAGE_ROS_VERSION == 1
ros::NodeHandle& n_;
Expand Down
2 changes: 2 additions & 0 deletions mvsim_node_src/mvsim_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -242,13 +242,15 @@ MVSimNode::MVSimNode(rclcpp::Node::SharedPtr& n)
void MVSimNode::launch_mvsim_server()
{
ROS12_INFO("[MVSimNode] launch_mvsim_server()");
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)

ASSERT_(!mvsim_server_);

// Start network server:
mvsim_server_ = mvsim_node::make_shared<mvsim::Server>();

mvsim_server_->start();
#endif
}

void MVSimNode::loadWorldModel(const std::string& world_xml_file)
Expand Down

0 comments on commit 46974eb

Please sign in to comment.