Program Listing for File robot_interaction.h
↰ Return to documentation for file (include/moveit/robot_interaction/robot_interaction.h
)
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Ioan Sucan, Adam Leeper */
#pragma once
#include <visualization_msgs/msg/interactive_marker_feedback.hpp>
#include <visualization_msgs/msg/interactive_marker.hpp>
#include <interactive_markers/menu_handler.hpp>
#include <moveit/macros/class_forward.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_interaction/interaction.h>
#include <rclcpp/logger.hpp>
#include <memory>
#include <functional>
#include <thread>
// This is needed for legacy code that includes robot_interaction.h but not
// interaction_handler.h
#include <moveit/robot_interaction/interaction_handler.h>
namespace interactive_markers
{
class InteractiveMarkerServer;
}
namespace robot_interaction
{
MOVEIT_CLASS_FORWARD(InteractionHandler); // Defines InteractionHandlerPtr, ConstPtr, WeakPtr... etc
MOVEIT_CLASS_FORWARD(KinematicOptionsMap); // Defines KinematicOptionsMapPtr, ConstPtr, WeakPtr... etc
MOVEIT_CLASS_FORWARD(RobotInteraction); // Defines RobotInteractionPtr, ConstPtr, WeakPtr... etc
// Manage interactive markers for controlling a robot state.
//
// The RobotInteraction class manages one or more InteractionHandler objects
// each of which maintains a set of interactive markers for manipulating one
// group of one RobotState.
//
// The group being manipulated is common to all InteractionHandler objects
// contained in a RobotInteraction instance.
class RobotInteraction
{
public:
static const std::string INTERACTIVE_MARKER_TOPIC;
RobotInteraction(const moveit::core::RobotModelConstPtr& robot_model, const rclcpp::Node::SharedPtr& node,
const std::string& ns = "");
virtual ~RobotInteraction();
const std::string& getServerTopic() const
{
return topic_;
}
void addActiveComponent(const InteractiveMarkerConstructorFn& construct, const ProcessFeedbackFn& process,
const InteractiveMarkerUpdateFn& update = InteractiveMarkerUpdateFn(),
const std::string& name = "");
void decideActiveComponents(const std::string& group);
void decideActiveComponents(const std::string& group, InteractionStyle::InteractionStyle style);
void clear();
void addInteractiveMarkers(const InteractionHandlerPtr& handler, const double marker_scale = 0.0);
// Update pose of all interactive markers to match the handler's RobotState.
// Call this when the handler's RobotState changes.
void updateInteractiveMarkers(const InteractionHandlerPtr& handler);
// True if markers are being shown for this handler.
bool showingMarkers(const InteractionHandlerPtr& handler);
// Display all markers that have been added.
// This is needed after calls to addInteractiveMarkers() to publish the
// resulting markers so they get displayed. This call is not needed after
// calling updateInteractiveMarkers() which publishes the results itself.
void publishInteractiveMarkers();
// Clear all interactive markers.
// This removes all interactive markers but does not affect which
// interactions are active. After this a call to publishInteractiveMarkers()
// is needed to actually remove the markers from the display.
void clearInteractiveMarkers();
const moveit::core::RobotModelConstPtr& getRobotModel() const
{
return robot_model_;
}
// Get the kinematic options map.
// Use this to set kinematic options (defaults or per-group).
KinematicOptionsMapPtr getKinematicOptionsMap()
{
return kinematic_options_map_;
}
// enable/disable subscription of the topics to move interactive marker
void toggleMoveInteractiveMarkerTopic(bool enable);
const std::vector<EndEffectorInteraction>& getActiveEndEffectors() const
{
return active_eef_;
}
const std::vector<JointInteraction>& getActiveJoints() const
{
return active_vj_;
}
private:
// called by decideActiveComponents(); add markers for end effectors
void decideActiveEndEffectors(const std::string& group);
void decideActiveEndEffectors(const std::string& group, InteractionStyle::InteractionStyle style);
// called by decideActiveComponents(); add markers for planar and floating joints
void decideActiveJoints(const std::string& group);
void moveInteractiveMarker(const std::string& name, const geometry_msgs::msg::PoseStamped& msg);
// register the name of the topic and marker name to move interactive marker from other ROS nodes
void registerMoveInteractiveMarkerTopic(const std::string& marker_name, const std::string& name);
// return the diameter of the sphere that certainly can enclose the AABB of the link
double computeLinkMarkerSize(const std::string& link);
// return the diameter of the sphere that certainly can enclose the AABB of the links in this group
double computeGroupMarkerSize(const std::string& group);
void computeMarkerPose(const InteractionHandlerPtr& handler, const EndEffectorInteraction& eef,
const moveit::core::RobotState& robot_state, geometry_msgs::msg::Pose& pose,
geometry_msgs::msg::Pose& control_to_eef_tf) const;
void addEndEffectorMarkers(const InteractionHandlerPtr& handler, const EndEffectorInteraction& eef,
visualization_msgs::msg::InteractiveMarker& im, bool position = true,
bool orientation = true);
void addEndEffectorMarkers(const InteractionHandlerPtr& handler, const EndEffectorInteraction& eef,
const geometry_msgs::msg::Pose& offset, visualization_msgs::msg::InteractiveMarker& im,
bool position = true, bool orientation = true);
void
processInteractiveMarkerFeedback(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback);
void subscribeMoveInteractiveMarker(const std::string marker_name, const std::string& name);
void processingThread();
void clearInteractiveMarkersUnsafe();
std::unique_ptr<std::thread> processing_thread_;
bool run_processing_thread_;
std::condition_variable new_feedback_condition_;
std::map<std::string, visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr> feedback_map_;
moveit::core::RobotModelConstPtr robot_model_;
std::vector<EndEffectorInteraction> active_eef_;
std::vector<JointInteraction> active_vj_;
std::vector<GenericInteraction> active_generic_;
std::map<std::string, InteractionHandlerPtr> handlers_;
std::map<std::string, std::size_t> shown_markers_;
// This mutex is locked every time markers are read or updated;
// This includes the active_* arrays and shown_markers_
// Please note that this mutex *MUST NOT* be locked while operations
// on the interactive marker server are called because the server
// also locks internally and we could othewrise end up with a problem
// of Thread 1: Lock A, Lock B, Unlock B, Unloack A
// Thread 2: Lock B, Lock A
// => deadlock
std::mutex marker_access_lock_;
interactive_markers::InteractiveMarkerServer* int_marker_server_;
// ros subscribers to move the interactive markers by other ros nodes
std::vector<rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr> int_marker_move_subscribers_;
// the array of the names of the topics which need to be subscribed
// to move the interactive markers by other ROS nodes
std::vector<std::string> int_marker_move_topics_;
// the array of the marker names in the same order to int_marker_move_topics_
std::vector<std::string> int_marker_names_;
std::string topic_;
rclcpp::Node::SharedPtr node_;
rclcpp::Logger logger_;
// options for doing IK
KinematicOptionsMapPtr kinematic_options_map_;
};
} // namespace robot_interaction