Skip to content

[WIP] Rough port of interactive marker server to Dashing #41

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
sloretz wants to merge 1 commit into from
Closed

[WIP] Rough port of interactive marker server to Dashing #41

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
114 changes: 58 additions & 56 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,67 +1,69 @@
cmake_minimum_required(VERSION 2.8.3)
cmake_minimum_required(VERSION 3.5)
project(interactive_markers)
find_package(catkin REQUIRED
rosconsole
roscpp
rospy
rostest
std_msgs
tf
visualization_msgs
)
catkin_package(
INCLUDE_DIRS include
LIBRARIES interactive_markers
CATKIN_DEPENDS roscpp rosconsole rospy tf visualization_msgs
)
catkin_python_setup()

include_directories(include ${catkin_INCLUDE_DIRS})
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(visualization_msgs REQUIRED)

add_library(${PROJECT_NAME}
src/interactive_marker_server.cpp
src/tools.cpp
src/menu_handler.cpp
src/interactive_marker_client.cpp
src/single_client.cpp
src/message_context.cpp
src/interactive_marker_server.cpp
# src/tools.cpp
# src/menu_handler.cpp
# src/interactive_marker_client.cpp
# src/single_client.cpp
# src/message_context.cpp
)

target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
target_include_directories(${PROJECT_NAME} PUBLIC include)
ament_target_dependencies(${PROJECT_NAME}
"rclcpp"
"tf2_ros"
"visualization_msgs")

install(TARGETS ${PROJECT_NAME}
DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
install(DIRECTORY include/interactive_markers/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h")
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)

# C++ Unit Tests
#
if(CATKIN_ENABLE_TESTING)
include_directories(${GTEST_INCLUDE_DIRS})
install(
DIRECTORY include/
DESTINATION include)

add_executable(server_test EXCLUDE_FROM_ALL src/test/server_test.cpp)
target_link_libraries(server_test ${PROJECT_NAME} ${GTEST_LIBRARIES})
add_dependencies(tests server_test)
add_rostest(test/cpp_server.test)
ament_export_dependencies("rclcpp")
ament_export_dependencies("tf2_ros")
ament_export_dependencies("visualization_msgs")
ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME})

add_executable(client_test EXCLUDE_FROM_ALL src/test/client_test.cpp)
target_link_libraries(client_test ${PROJECT_NAME} ${GTEST_LIBRARIES})
add_dependencies(tests client_test)
add_rostest(test/cpp_client.test)
ament_package()

add_executable(server_client_test EXCLUDE_FROM_ALL src/test/server_client_test.cpp)
target_link_libraries(server_client_test ${PROJECT_NAME} ${GTEST_LIBRARIES})
add_dependencies(tests server_client_test)
add_rostest(test/cpp_server_client.test)

# Test program to simulate Interactive Marker with missing tf information
add_executable(bursty_tf EXCLUDE_FROM_ALL src/test/bursty_tf.cpp)
target_link_libraries(bursty_tf ${PROJECT_NAME})
add_dependencies(tests bursty_tf)

# Test program to simulate Interactive Marker with wrong tf information
add_executable(missing_tf EXCLUDE_FROM_ALL src/test/missing_tf.cpp)
target_link_libraries(missing_tf ${PROJECT_NAME})
add_dependencies(tests missing_tf)
endif()
# C++ Unit Tests
#
# if(CATKIN_ENABLE_TESTING)
# include_directories(${GTEST_INCLUDE_DIRS})
#
# add_executable(server_test EXCLUDE_FROM_ALL src/test/server_test.cpp)
# target_link_libraries(server_test ${PROJECT_NAME} ${GTEST_LIBRARIES})
# add_dependencies(tests server_test)
# add_rostest(test/cpp_server.test)
#
# add_executable(client_test EXCLUDE_FROM_ALL src/test/client_test.cpp)
# target_link_libraries(client_test ${PROJECT_NAME} ${GTEST_LIBRARIES})
# add_dependencies(tests client_test)
# add_rostest(test/cpp_client.test)
#
# add_executable(server_client_test EXCLUDE_FROM_ALL src/test/server_client_test.cpp)
# target_link_libraries(server_client_test ${PROJECT_NAME} ${GTEST_LIBRARIES})
# add_dependencies(tests server_client_test)
# add_rostest(test/cpp_server_client.test)
#
# # Test program to simulate Interactive Marker with missing tf information
# add_executable(bursty_tf EXCLUDE_FROM_ALL src/test/bursty_tf.cpp)
# target_link_libraries(bursty_tf ${PROJECT_NAME})
# add_dependencies(tests bursty_tf)
#
# # Test program to simulate Interactive Marker with wrong tf information
# add_executable(missing_tf EXCLUDE_FROM_ALL src/test/missing_tf.cpp)
# target_link_libraries(missing_tf ${PROJECT_NAME})
# add_dependencies(tests missing_tf)
# endif()
116 changes: 69 additions & 47 deletions include/interactive_markers/interactive_marker_server.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,19 +32,27 @@
#ifndef INTERACTIVE_MARKER_SERVER
#define INTERACTIVE_MARKER_SERVER

#include <visualization_msgs/InteractiveMarkerUpdate.h>
#include <visualization_msgs/InteractiveMarkerFeedback.h>
#include <visualization_msgs/msg/interactive_marker_init.hpp>
#include <visualization_msgs/msg/interactive_marker_feedback.hpp>
#include <visualization_msgs/msg/interactive_marker_update.hpp>

#include <boost/scoped_ptr.hpp>
#include <boost/thread/thread.hpp>
#include <boost/thread/recursive_mutex.hpp>
#include <functional>
#include <mutex>
#include <thread>
#include <unordered_map>

#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <rclcpp/rclcpp.hpp>

// #include <boost/scoped_ptr.hpp>
// #include <boost/thread/thread.hpp>
// #include <boost/thread/recursive_mutex.hpp>

#include <boost/function.hpp>
#include <boost/unordered_map.hpp>
// #include <ros/ros.h>
// #include <ros/callback_queue.h>
//
//
// #include <boost/function.hpp>
// #include <boost/unordered_map.hpp>

namespace interactive_markers
{
Expand All @@ -53,12 +61,12 @@ namespace interactive_markers
///
/// Note: Keep in mind that changes made by calling insert(), erase(), setCallback() etc.
/// are not applied until calling applyChanges().
class InteractiveMarkerServer : boost::noncopyable
class InteractiveMarkerServer
{
public:

typedef visualization_msgs::InteractiveMarkerFeedbackConstPtr FeedbackConstPtr;
typedef boost::function< void ( const FeedbackConstPtr& ) > FeedbackCallback;
typedef visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr FeedbackConstPtr;
typedef std::function< void ( FeedbackConstPtr ) > FeedbackCallback;

static const uint8_t DEFAULT_FEEDBACK_CB = 255;

Expand All @@ -67,9 +75,27 @@ class InteractiveMarkerServer : boost::noncopyable
/// @param server_id If you run multiple servers on the same topic from
/// within the same node, you will need to assign different names to them.
/// Otherwise, leave this empty.
/// @param spin_thread If set to true, will spin up a thread for message handling.
/// All callbacks will be called from that thread.
InteractiveMarkerServer( const std::string &topic_ns, const std::string &server_id="", bool spin_thread = false );
InteractiveMarkerServer( const std::string &topic_ns,
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr base_interface,
rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface,
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface,
rclcpp::node_interfaces::NodeTimersInterface::SharedPtr timers_interface,
rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface,
const std::string server_id);

template <typename NodePtr>
InteractiveMarkerServer(const std::string &topic_ns, NodePtr node, const std::string &server_id="")
:
InteractiveMarkerServer(
topic_ns,
node->get_node_base_interface(),
node->get_node_clock_interface(),
node->get_node_logging_interface(),
node->get_node_timers_interface(),
node->get_node_topics_interface(),
server_id)
{
}

/// Destruction of the interface will lead to all managed markers being cleared.
~InteractiveMarkerServer();
Expand All @@ -78,15 +104,15 @@ class InteractiveMarkerServer : boost::noncopyable
/// Note: Changes to the marker will not take effect until you call applyChanges().
/// The callback changes immediately.
/// @param int_marker The marker to be added or replaced
void insert( const visualization_msgs::InteractiveMarker &int_marker );
void insert( const visualization_msgs::msg::InteractiveMarker &int_marker );

/// Add or replace a marker and its callback functions
/// Note: Changes to the marker will not take effect until you call applyChanges().
/// The callback changes immediately.
/// @param int_marker The marker to be added or replaced
/// @param feedback_cb Function to call on the arrival of a feedback message.
/// @param feedback_type Type of feedback for which to call the feedback.
void insert( const visualization_msgs::InteractiveMarker &int_marker,
void insert( const visualization_msgs::msg::InteractiveMarker &int_marker,
FeedbackCallback feedback_cb,
uint8_t feedback_type=DEFAULT_FEEDBACK_CB );

Expand All @@ -97,8 +123,8 @@ class InteractiveMarkerServer : boost::noncopyable
/// @param pose The new pose
/// @param header Header replacement. Leave this empty to use the previous one.
bool setPose( const std::string &name,
const geometry_msgs::Pose &pose,
const std_msgs::Header &header=std_msgs::Header() );
const geometry_msgs::msg::Pose &pose,
const std_msgs::msg::Header &header=std_msgs::msg::Header() );

/// Erase the marker with the specified name
/// Note: This change will not take effect until you call applyChanges().
Expand Down Expand Up @@ -141,20 +167,23 @@ class InteractiveMarkerServer : boost::noncopyable
/// @param name Name of the interactive marker
/// @param[out] int_marker Output message
/// @return true if a marker with that name exists
bool get( std::string name, visualization_msgs::InteractiveMarker &int_marker ) const;
bool get( std::string name, visualization_msgs::msg::InteractiveMarker &int_marker ) const;

private:
// Disable copying
InteractiveMarkerServer(const InteractiveMarkerServer &) = delete;
InteractiveMarkerServer & operator=(const InteractiveMarkerServer &) = delete;

struct MarkerContext
{
ros::Time last_feedback;
rclcpp::Time last_feedback;
std::string last_client_id;
FeedbackCallback default_feedback_cb;
boost::unordered_map<uint8_t,FeedbackCallback> feedback_cbs;
visualization_msgs::InteractiveMarker int_marker;
std::unordered_map<uint8_t,FeedbackCallback> feedback_cbs;
visualization_msgs::msg::InteractiveMarker int_marker;
};

typedef boost::unordered_map< std::string, MarkerContext > M_MarkerContext;
typedef std::unordered_map< std::string, MarkerContext > M_MarkerContext;

// represents an update to a single marker
struct UpdateContext
Expand All @@ -164,35 +193,30 @@ class InteractiveMarkerServer : boost::noncopyable
POSE_UPDATE,
ERASE
} update_type;
visualization_msgs::InteractiveMarker int_marker;
visualization_msgs::msg::InteractiveMarker int_marker;
FeedbackCallback default_feedback_cb;
boost::unordered_map<uint8_t,FeedbackCallback> feedback_cbs;
std::unordered_map<uint8_t,FeedbackCallback> feedback_cbs;
};

typedef boost::unordered_map< std::string, UpdateContext > M_UpdateContext;

// main loop when spinning our own thread
// - process callbacks in our callback queue
// - process pending goals
void spinThread();
typedef std::unordered_map< std::string, UpdateContext > M_UpdateContext;

// update marker pose & call user callback
void processFeedback( const FeedbackConstPtr& feedback );
void processFeedback( visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr feedback );

// send an empty update to keep the client GUIs happy
void keepAlive();

// increase sequence number & publish an update
void publish( visualization_msgs::InteractiveMarkerUpdate &update );
void publish( visualization_msgs::msg::InteractiveMarkerUpdate &update );

// publish the current complete state to the latched "init" topic.
void publishInit();

// Update pose, schedule update without locking
void doSetPose( M_UpdateContext::iterator update_it,
const std::string &name,
const geometry_msgs::Pose &pose,
const std_msgs::Header &header );
const geometry_msgs::msg::Pose &pose,
const std_msgs::msg::Header &header );

// contains the current state of all markers
M_MarkerContext marker_contexts_;
Expand All @@ -203,20 +227,18 @@ class InteractiveMarkerServer : boost::noncopyable
// topic namespace to use
std::string topic_ns_;

mutable boost::recursive_mutex mutex_;

// these are needed when spinning up a dedicated thread
boost::scoped_ptr<boost::thread> spin_thread_;
ros::NodeHandle node_handle_;
ros::CallbackQueue callback_queue_;
volatile bool need_to_terminate_;
mutable std::recursive_mutex mutex_;

// this is needed when running in non-threaded mode
ros::Timer keep_alive_timer_;
rclcpp::TimerBase::SharedPtr keep_alive_timer_;

rclcpp::Publisher<visualization_msgs::msg::InteractiveMarkerInit>::SharedPtr init_pub_;
rclcpp::Publisher<visualization_msgs::msg::InteractiveMarkerUpdate>::SharedPtr update_pub_;
rclcpp::Subscription<visualization_msgs::msg::InteractiveMarkerFeedback>::SharedPtr feedback_sub_;

ros::Publisher init_pub_;
ros::Publisher update_pub_;
ros::Subscriber feedback_sub_;
rclcpp::Context::SharedPtr context_;
rclcpp::Clock::SharedPtr clock_;
rclcpp::Logger logger_;

uint64_t seq_num_;

Expand Down
13 changes: 11 additions & 2 deletions package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
<package>
<package format="2">
<name>interactive_markers</name>
<description>
3D interactive marker communication library for RViz and similar tools.
Expand All @@ -11,7 +11,7 @@

<url>http://ros.org/wiki/interactive_markers</url>

<buildtool_depend>catkin</buildtool_depend>
<!--<buildtool_depend>catkin</buildtool_depend>

<build_depend>rosconsole</build_depend>
<build_depend>roscpp</build_depend>
Expand All @@ -28,4 +28,13 @@
<run_depend>std_msgs</run_depend>
<run_depend>tf</run_depend>
<run_depend>visualization_msgs</run_depend>
-->

<depend>rclcpp</depend>
<depend>tf2_ros</depend>
<depend>visualization_msgs</depend>

<export>
<build_type>ament_cmake</build_type>
</export>
</package>
Loading