diff --git a/MIGRATION.md b/MIGRATION.md index 0c17e6b517..f3a518b8c0 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -39,6 +39,8 @@ API changes in MoveIt releases - In case you start RViz in a namespace, the default topic for the trajectory visualization display now uses the relative instead of the absolute namespace (i.e. `/move_group/display_planned_path` instead of `/move_group/display_planned_path`). - `RobotState::attachBody()` now takes a unique_ptr instead of an owning raw pointer. - Moved the class `MoveItErrorCode` from both `moveit_ros_planning` and `moveit_ros_planning_interface` to `moveit_core`. The class now is in namespace `moveit::core`, access via `moveit::planning_interface` or `moveit_cpp::PlanningComponent` is deprecated. +- End-effector markers in rviz are shown only if the eef's parent group is active _and_ the parent link is part of that group. Before, these conditions were _OR_-connected. + You might need to define additional end-effectors. ## ROS Melodic diff --git a/moveit_commander/src/moveit_commander/move_group.py b/moveit_commander/src/moveit_commander/move_group.py index 8c980bccaf..9e6fbb36f3 100644 --- a/moveit_commander/src/moveit_commander/move_group.py +++ b/moveit_commander/src/moveit_commander/move_group.py @@ -603,7 +603,7 @@ def go(self, joints=None, wait=True): elif joints is not None: try: self.set_joint_value_target(self.get_remembered_joint_values()[joints]) - except TypeError: + except (KeyError, TypeError): self.set_joint_value_target(joints) if wait: return self._g.move() diff --git a/moveit_commander/test/python_moveit_commander.py b/moveit_commander/test/python_moveit_commander.py index 8dbd6e0946..75054d8a90 100755 --- a/moveit_commander/test/python_moveit_commander.py +++ b/moveit_commander/test/python_moveit_commander.py @@ -43,12 +43,21 @@ import os from moveit_msgs.msg import RobotState +from sensor_msgs.msg import JointState from moveit_commander import RobotCommander, PlanningSceneInterface class PythonMoveitCommanderTest(unittest.TestCase): PLANNING_GROUP = "manipulator" + JOINT_NAMES = [ + "joint_1", + "joint_2", + "joint_3", + "joint_4", + "joint_5", + "joint_6", + ] @classmethod def setUpClass(self): @@ -67,14 +76,7 @@ def test_enforce_bounds_empty_state(self): def test_enforce_bounds(self): in_msg = RobotState() in_msg.joint_state.header.frame_id = "base_link" - in_msg.joint_state.name = [ - "joint_1", - "joint_2", - "joint_3", - "joint_4", - "joint_5", - "joint_6", - ] + in_msg.joint_state.name = self.JOINT_NAMES in_msg.joint_state.position = [0] * 6 in_msg.joint_state.position[0] = 1000 @@ -87,14 +89,7 @@ def test_get_current_state(self): expected_state = RobotState() expected_state.joint_state.header.frame_id = "base_link" expected_state.multi_dof_joint_state.header.frame_id = "base_link" - expected_state.joint_state.name = [ - "joint_1", - "joint_2", - "joint_3", - "joint_4", - "joint_5", - "joint_6", - ] + expected_state.joint_state.name = self.JOINT_NAMES expected_state.joint_state.position = [0] * 6 self.assertEqual(self.group.get_current_state(), expected_state) @@ -141,6 +136,25 @@ def test_validation(self): self.assertTrue(success3) self.assertTrue(self.group.execute(plan3)) + def test_gogogo(self): + current_joints = np.asarray(self.group.get_current_joint_values()) + + self.group.set_joint_value_target(current_joints) + self.assertTrue(self.group.go(True)) + + self.assertTrue(self.group.go(current_joints)) + self.assertTrue(self.group.go(list(current_joints))) + self.assertTrue(self.group.go(tuple(current_joints))) + self.assertTrue( + self.group.go(JointState(name=self.JOINT_NAMES, position=current_joints)) + ) + + self.group.remember_joint_values("current") + self.assertTrue(self.group.go("current")) + + current_pose = self.group.get_current_pose() + self.assertTrue(self.group.go(current_pose)) + def test_planning_scene_interface(self): planning_scene = PlanningSceneInterface() diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp index 41732cb0d6..33b0d3c482 100644 --- a/moveit_core/collision_detection/src/collision_matrix.cpp +++ b/moveit_core/collision_detection/src/collision_matrix.cpp @@ -344,10 +344,14 @@ void AllowedCollisionMatrix::getAllEntryNames(std::vector& names) c { names.clear(); for (const auto& entry : entries_) - if (!names.empty() && names.back() == entry.first) - continue; - else - names.push_back(entry.first); + names.push_back(entry.first); + + for (const auto& item : default_entries_) + { + auto it = std::lower_bound(names.begin(), names.end(), item.first); + if (it != names.end() && *it != item.first) + names.insert(it, item.first); + } } void AllowedCollisionMatrix::getMessage(moveit_msgs::msg::AllowedCollisionMatrix& msg) const @@ -358,7 +362,6 @@ void AllowedCollisionMatrix::getMessage(moveit_msgs::msg::AllowedCollisionMatrix msg.default_entry_values.clear(); getAllEntryNames(msg.entry_names); - std::sort(msg.entry_names.begin(), msg.entry_names.end()); msg.entry_values.resize(msg.entry_names.size()); for (std::size_t i = 0; i < msg.entry_names.size(); ++i) @@ -389,7 +392,6 @@ void AllowedCollisionMatrix::print(std::ostream& out) const { std::vector names; getAllEntryNames(names); - std::sort(names.begin(), names.end()); std::size_t spacing = 4; for (auto& name : names) diff --git a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp index 9dece47a2d..22a542a6fd 100644 --- a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp +++ b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp @@ -51,17 +51,26 @@ std::vector determineCollisionSpheres(const bodies::Body* body, { std::vector css; - bodies::BoundingCylinder cyl; - body->computeBoundingCylinder(cyl); - unsigned int num_points = ceil(cyl.length / (cyl.radius / 2.0)); - double spacing = cyl.length / ((num_points * 1.0) - 1.0); - relative_transform = cyl.pose; - - for (unsigned int i = 1; i < num_points - 1; ++i) + if (body->getType() == shapes::ShapeType::SPHERE) { - CollisionSphere cs(relative_transform * Eigen::Vector3d(0, 0, (-cyl.length / 2.0) + i * spacing), cyl.radius); + collision_detection::CollisionSphere cs(body->getPose().translation(), body->getDimensions()[0]); css.push_back(cs); } + else + { + bodies::BoundingCylinder cyl; + body->computeBoundingCylinder(cyl); + unsigned int num_points = ceil(cyl.length / (cyl.radius / 2.0)); + double spacing = cyl.length / ((num_points * 1.0) - 1.0); + relative_transform = cyl.pose; + + for (unsigned int i = 1; i < num_points - 1; i++) + { + collision_detection::CollisionSphere cs( + relative_transform * Eigen::Vector3d(0, 0, (-cyl.length / 2.0) + i * spacing), cyl.radius); + css.push_back(cs); + } + } return css; } diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h index d44b0232cf..c0c7dc848d 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h @@ -44,6 +44,7 @@ namespace trajectory_processing { /// \brief This class modifies the timestamps of a trajectory to respect /// velocity and acceleration constraints. +MOVEIT_CLASS_FORWARD(IterativeParabolicTimeParameterization); class IterativeParabolicTimeParameterization : public TimeParameterization { public: diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h index b5f35b162a..ff2805636e 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h @@ -160,6 +160,7 @@ class Trajectory mutable std::list::const_iterator cached_trajectory_segment_; }; +MOVEIT_CLASS_FORWARD(TimeOptimalTrajectoryGeneration); class TimeOptimalTrajectoryGeneration : public TimeParameterization { public: diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h index 001e8e8543..4746e11967 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h @@ -7,6 +7,7 @@ namespace trajectory_processing /** * @brief Base class for trajectory parameterization algorithms */ +MOVEIT_CLASS_FORWARD(TimeParameterization); class TimeParameterization { public: diff --git a/moveit_ros/benchmarks/scripts/moveit_benchmark_statistics.py b/moveit_ros/benchmarks/scripts/moveit_benchmark_statistics.py index 6f34144389..f2890c03d7 100755 --- a/moveit_ros/benchmarks/scripts/moveit_benchmark_statistics.py +++ b/moveit_ros/benchmarks/scripts/moveit_benchmark_statistics.py @@ -112,6 +112,9 @@ def readRequiredMultilineValue(filevar): def readBenchmarkLog(dbname, filenames): """Parse benchmark log files and store the parsed data in a sqlite3 database.""" + def isInvalidValue(value): + return len(value) == 0 or value in ["nan", "-nan", "inf", "-inf"] + conn = sqlite3.connect(dbname) c = conn.cursor() c.execute("PRAGMA FOREIGN_KEYS = ON") @@ -313,7 +316,7 @@ def readBenchmarkLog(dbname, filenames): runIds = [] for j in range(numRuns): runValues = [ - None if len(x) == 0 or x == "nan" or x == "inf" else x + None if isInvalidValue(x) else x for x in logfile.readline().split("; ")[:-1] ] values = tuple([experimentId, plannerId] + runValues) @@ -362,7 +365,7 @@ def readBenchmarkLog(dbname, filenames): values = tuple( [runIds[j]] + [ - None if len(x) == 0 or x == "nan" or x == "inf" else x + None if isInvalidValue(x) else x for x in dataSample.split(",")[:-1] ] ) diff --git a/moveit_ros/move_group/include/moveit/move_group/move_group_context.h b/moveit_ros/move_group/include/moveit/move_group/move_group_context.h index ff5f8f035a..5d37a509dd 100644 --- a/moveit_ros/move_group/include/moveit/move_group/move_group_context.h +++ b/moveit_ros/move_group/include/moveit/move_group/move_group_context.h @@ -37,6 +37,7 @@ #pragma once #include +#include #include namespace moveit_cpp diff --git a/moveit_ros/move_group/src/move_group.cpp b/moveit_ros/move_group/src/move_group.cpp index 09aadc7d0c..c44bd7e47f 100644 --- a/moveit_ros/move_group/src/move_group.cpp +++ b/moveit_ros/move_group/src/move_group.cpp @@ -236,16 +236,16 @@ int main(int argc, char** argv) std::string default_planning_pipeline; if (nh->get_parameter("default_planning_pipeline", default_planning_pipeline)) { - // Ignore default_planning_pipeline if there is no known entry in pipeline_names + // Ignore default_planning_pipeline if there is no matching entry in pipeline_names if (std::find(pipeline_names.begin(), pipeline_names.end(), default_planning_pipeline) == pipeline_names.end()) { RCLCPP_WARN(LOGGER, - "MoveGroup launched with ~default_planning_pipeline '%s' not configured in ~/planning_pipelines", + "MoveGroup launched with ~default_planning_pipeline '%s' not configured in ~planning_pipelines", default_planning_pipeline.c_str()); default_planning_pipeline = ""; // reset invalid pipeline id } } - else + else if (pipeline_names.size() > 1) // only warn if there are multiple pipelines to choose from { // Handle deprecated move_group.launch RCLCPP_WARN(LOGGER, diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp index b6c6b12d15..4dad5c835d 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -294,7 +294,7 @@ void RobotInteraction::decideActiveEndEffectors(const std::string& group, Intera auto add_active_end_effectors_for_single_group = [&](const moveit::core::JointModelGroup* single_group) { bool found_eef{ false }; for (const srdf::Model::EndEffector& eef : eefs) - if ((single_group->hasLinkModel(eef.parent_link_) || single_group->getName() == eef.parent_group_) && + if (single_group->hasLinkModel(eef.parent_link_) && single_group->getName() == eef.parent_group_ && single_group->canSetStateFromIK(eef.parent_link_)) { // We found an end-effector whose parent is the group. diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 697aa214b3..f6fb62e776 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -619,6 +619,13 @@ void MotionPlanningFrame::initFromMoveGroupNS() planning_scene_world_publisher_ = node_->create_publisher("planning_scene_world", 1); + // Declare parameter for default planning pipeline + if (!node_->has_parameter(planning_display_->getMoveGroupNS() + "default_planning_pipeline")) + node_->declare_parameter(planning_display_->getMoveGroupNS() + "default_planning_pipeline", ""); + + // Query default planning pipeline id + node_->get_parameter(planning_display_->getMoveGroupNS() + "default_planning_pipeline", default_planning_pipeline_); + // Set initial velocity and acceleration scaling factors from ROS parameters double factor; node_->get_parameter_or("robot_description_planning.default_velocity_scaling_factor", factor, 0.1); @@ -638,10 +645,6 @@ void MotionPlanningFrame::initFromMoveGroupNS() { ui_->database_port->setValue(port); } - - // Query default planning pipeline id - node_->get_parameter(planning_display_->getMoveGroupNS() + "/move_group/default_planning_pipeline", - default_planning_pipeline_); } void MotionPlanningFrame::disable() diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h index 0cad7c6bd2..f9b3b9d345 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h @@ -87,7 +87,7 @@ class TrajectoryVisualization : public QObject ~TrajectoryVisualization() override; - virtual void update(float wall_dt, float ros_dt); + virtual void update(float wall_dt, float sim_dt); virtual void reset(); void onInitialize(const rclcpp::Node::SharedPtr& node, Ogre::SceneNode* scene_node, @@ -172,6 +172,7 @@ private Q_SLOTS: rviz_common::properties::RosTopicProperty* trajectory_topic_property_; rviz_common::properties::FloatProperty* robot_path_alpha_property_; rviz_common::properties::BoolProperty* loop_display_property_; + rviz_common::properties::BoolProperty* use_sim_time_property_; rviz_common::properties::BoolProperty* trail_display_property_; rviz_common::properties::BoolProperty* interrupt_display_property_; rviz_common::properties::ColorProperty* robot_color_property_; diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp index 3cc2322ae4..c31f1d1068 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp @@ -108,6 +108,10 @@ TrajectoryVisualization::TrajectoryVisualization(rviz_common::properties::Proper state_display_time_property_->addOptionStd("0.1s"); state_display_time_property_->addOptionStd("0.5s"); + use_sim_time_property_ = new rviz_common::properties::BoolProperty( + "Use Sim Time", false, "Indicates whether simulation time or wall-time is used for state display timing.", widget, + nullptr, this); + loop_display_property_ = new rviz_common::properties::BoolProperty("Loop Animation", false, "Indicates whether the last received path " "is to be animated in a loop", @@ -410,7 +414,7 @@ void TrajectoryVisualization::dropTrajectory() drop_displaying_trajectory_ = true; } -void TrajectoryVisualization::update(float wall_dt, float /*ros_dt*/) +void TrajectoryVisualization::update(float wall_dt, float sim_dt) { if (drop_displaying_trajectory_) { @@ -460,7 +464,14 @@ void TrajectoryVisualization::update(float wall_dt, float /*ros_dt*/) { int previous_state = current_state_; int waypoint_count = displaying_trajectory_message_->getWayPointCount(); - current_state_time_ += wall_dt; + if (use_sim_time_property_->getBool()) + { + current_state_time_ += sim_dt; + } + else + { + current_state_time_ += wall_dt; + } float tm = getStateDisplayTime(); if (trajectory_slider_panel_ && trajectory_slider_panel_->isVisible() && trajectory_slider_panel_->isPaused()) diff --git a/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py b/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py index 939bccae5c..864c9c29bb 100644 --- a/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py +++ b/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py @@ -94,74 +94,23 @@ def __init__(self): class XBoxStatus(JoyStatus): def __init__(self, msg): JoyStatus.__init__(self) - if msg.buttons[8] == 1: - self.center = True - else: - self.center = False - if msg.buttons[6] == 1: - self.select = True - else: - self.select = False - if msg.buttons[7] == 1: - self.start = True - else: - self.start = False - if msg.buttons[9] == 1: - self.L3 = True - else: - self.L3 = False - if msg.buttons[10] == 1: - self.R3 = True - else: - self.R3 = False - if msg.buttons[2] == 1: - self.square = True - else: - self.square = False - if msg.buttons[1] == 1: - self.circle = True - else: - self.circle = False - if msg.axes[7] > 0.1: - self.up = True - else: - self.up = False - if msg.axes[7] < -0.1: - self.down = True - else: - self.down = False - if msg.axes[6] > 0.1: - self.left = True - else: - self.left = False - if msg.axes[6] < -0.1: - self.right = True - else: - self.right = False - if msg.buttons[3] == 1: - self.triangle = True - else: - self.triangle = False - if msg.buttons[0] == 1: - self.cross = True - else: - self.cross = False - if msg.buttons[4] == 1: - self.L1 = True - else: - self.L1 = False - if msg.buttons[5] == 1: - self.R1 = True - else: - self.R1 = False - if msg.axes[2] < -0.5: - self.L2 = True - else: - self.L2 = False - if msg.axes[5] < -0.5: - self.R2 = True - else: - self.R2 = False + self.center = msg.buttons[8] == 1 + self.select = msg.buttons[6] == 1 + self.start = msg.buttons[7] == 1 + self.L3 = msg.buttons[9] == 1 + self.R3 = msg.buttons[10] == 1 + self.square = msg.buttons[2] == 1 + self.circle = msg.buttons[1] == 1 + self.up = msg.axes[7] > 0.1 + self.down = msg.axes[7] < -0.1 + self.left = msg.axes[6] > 0.1 + self.right = msg.axes[6] < -0.1 + self.triangle = msg.buttons[3] == 1 + self.cross = msg.buttons[0] == 1 + self.L1 = msg.buttons[4] == 1 + self.R1 = msg.buttons[5] == 1 + self.L2 = msg.axes[2] < -0.5 + self.R2 = msg.axes[5] < -0.5 self.left_analog_x = msg.axes[0] self.left_analog_y = msg.axes[1] self.right_analog_x = msg.axes[3] @@ -169,78 +118,50 @@ def __init__(self, msg): self.orig_msg = msg +class PS3DualShockStatus(JoyStatus): + def __init__(self, msg): + JoyStatus.__init__(self) + # creating from sensor_msgs/Joy + self.cross = msg.buttons[0] == 1 + self.circle = msg.buttons[1] == 1 + self.triangle = msg.buttons[2] == 1 + self.square = msg.buttons[3] == 1 + self.L1 = msg.buttons[4] == 1 + self.R1 = msg.buttons[5] == 1 + self.L2 = msg.buttons[6] == 1 + self.R2 = msg.buttons[7] == 1 + self.select = msg.buttons[8] == 1 + self.start = msg.buttons[9] == 1 + self.center = msg.buttons[10] == 1 + self.left_analog_x = msg.axes[0] + self.left_analog_y = msg.axes[1] + self.right_analog_x = msg.axes[3] + self.right_analog_y = msg.axes[4] + + self.orig_msg = msg + + class PS3Status(JoyStatus): def __init__(self, msg): JoyStatus.__init__(self) # creating from sensor_msgs/Joy - if msg.buttons[16] == 1: - self.center = True - else: - self.center = False - if msg.buttons[0] == 1: - self.select = True - else: - self.select = False - if msg.buttons[3] == 1: - self.start = True - else: - self.start = False - if msg.buttons[1] == 1: - self.L3 = True - else: - self.L3 = False - if msg.buttons[2] == 1: - self.R3 = True - else: - self.R3 = False - if msg.axes[15] < 0: - self.square = True - else: - self.square = False - if msg.axes[4] < 0: - self.up = True - else: - self.up = False - if msg.axes[6] < 0: - self.down = True - else: - self.down = False - if msg.axes[7] < 0: - self.left = True - else: - self.left = False - if msg.axes[5] < 0: - self.right = True - else: - self.right = False - if msg.axes[12] < 0: - self.triangle = True - else: - self.triangle = False - if msg.axes[14] < 0: - self.cross = True - else: - self.cross = False - if msg.axes[13] < 0: - self.circle = True - else: - self.circle = False - if msg.axes[10] < 0: - self.L1 = True - else: - self.L1 = False - if msg.axes[11] < 0: - self.R1 = True - else: - self.R1 = False - if msg.axes[8] < 0: - self.L2 = True - else: - self.L2 = False - if msg.axes[9] < 0: - self.R2 = True - else: - self.R2 = False + self.center = msg.buttons[16] == 1 + self.select = msg.buttons[0] == 1 + self.start = msg.buttons[3] == 1 + self.L3 = msg.buttons[1] == 1 + self.R3 = msg.buttons[2] == 1 + self.square = msg.axes[15] < 0 + self.up = msg.axes[4] < 0 + self.down = msg.axes[6] < 0 + self.left = msg.axes[7] < 0 + self.right = msg.axes[5] < 0 + self.triangle = msg.axes[12] < 0 + self.cross = msg.axes[14] < 0 + self.circle = msg.axes[13] < 0 + self.L1 = msg.axes[10] < 0 + self.R1 = msg.axes[11] < 0 + self.L2 = msg.axes[8] < 0 + self.R2 = msg.axes[9] < 0 self.left_analog_x = msg.axes[0] self.left_analog_y = msg.axes[1] self.right_analog_x = msg.axes[2] @@ -252,74 +173,23 @@ class PS3WiredStatus(JoyStatus): def __init__(self, msg): JoyStatus.__init__(self) # creating from sensor_msgs/Joy - if msg.buttons[16] == 1: - self.center = True - else: - self.center = False - if msg.buttons[0] == 1: - self.select = True - else: - self.select = False - if msg.buttons[3] == 1: - self.start = True - else: - self.start = False - if msg.buttons[1] == 1: - self.L3 = True - else: - self.L3 = False - if msg.buttons[2] == 1: - self.R3 = True - else: - self.R3 = False - if msg.buttons[15] == 1: - self.square = True - else: - self.square = False - if msg.buttons[4] == 1: - self.up = True - else: - self.up = False - if msg.buttons[6] == 1: - self.down = True - else: - self.down = False - if msg.buttons[7] == 1: - self.left = True - else: - self.left = False - if msg.buttons[5] == 1: - self.right = True - else: - self.right = False - if msg.buttons[12] == 1: - self.triangle = True - else: - self.triangle = False - if msg.buttons[14] == 1: - self.cross = True - else: - self.cross = False - if msg.buttons[13] == 1: - self.circle = True - else: - self.circle = False - if msg.buttons[10] == 1: - self.L1 = True - else: - self.L1 = False - if msg.buttons[11] == 1: - self.R1 = True - else: - self.R1 = False - if msg.buttons[8] == 1: - self.L2 = True - else: - self.L2 = False - if msg.buttons[9] == 1: - self.R2 = True - else: - self.R2 = False + self.center = msg.buttons[16] == 1 + self.select = msg.buttons[0] == 1 + self.start = msg.buttons[3] == 1 + self.L3 = msg.buttons[1] == 1 + self.R3 = msg.buttons[2] == 1 + self.square = msg.buttons[15] == 1 + self.up = msg.buttons[4] == 1 + self.down = msg.buttons[6] == 1 + self.left = msg.buttons[7] == 1 + self.right = msg.buttons[5] == 1 + self.triangle = msg.buttons[12] == 1 + self.cross = msg.buttons[14] == 1 + self.circle = msg.buttons[13] == 1 + self.L1 = msg.buttons[10] == 1 + self.R1 = msg.buttons[11] == 1 + self.L2 = msg.buttons[8] == 1 + self.R2 = msg.buttons[9] == 1 self.left_analog_x = msg.axes[0] self.left_analog_y = msg.axes[1] self.right_analog_x = msg.axes[2] @@ -331,74 +201,23 @@ class PS4Status(JoyStatus): def __init__(self, msg): JoyStatus.__init__(self) # creating from sensor_msg/Joy - if msg.buttons[12] == 1: - self.center = True - else: - self.center = False - if msg.buttons[8] == 1: - self.select = True - else: - self.select = False - if msg.buttons[9] == 1: - self.start = True - else: - self.start = False - if msg.buttons[10] == 1: - self.L3 = True - else: - self.L3 = False - if msg.buttons[11] == 1: - self.R3 = True - else: - self.R3 = False - if msg.buttons[0] == 1: - self.square = True - else: - self.square = False - if msg.axes[10] < 0: - self.up = True - else: - self.up = False - if msg.axes[10] > 0: - self.down = True - else: - self.down = False - if msg.axes[9] < 0: - self.left = True - else: - self.left = False - if msg.axes[9] > 0: - self.right = True - else: - self.right = False - if msg.buttons[3] == 1: - self.triangle = True - else: - self.triangle = False - if msg.buttons[1] == 1: - self.cross = True - else: - self.cross = False - if msg.buttons[2] == 1: - self.circle = True - else: - self.circle = False - if msg.buttons[4] == 1: - self.L1 = True - else: - self.L1 = False - if msg.buttons[5] == 1: - self.R1 = True - else: - self.R1 = False - if msg.buttons[6] == 1: - self.L2 = True - else: - self.L2 = False - if msg.buttons[7] == 1: - self.R2 = True - else: - self.R2 = False + self.center = msg.buttons[12] == 1 + self.select = msg.buttons[8] == 1 + self.start = msg.buttons[9] == 1 + self.L3 = msg.buttons[10] == 1 + self.R3 = msg.buttons[11] == 1 + self.square = msg.buttons[0] == 1 + self.up = msg.axes[10] < 0 + self.down = msg.axes[10] > 0 + self.left = msg.axes[9] < 0 + self.right = msg.axes[9] > 0 + self.triangle = msg.buttons[3] == 1 + self.cross = msg.buttons[1] == 1 + self.circle = msg.buttons[2] == 1 + self.L1 = msg.buttons[4] == 1 + self.R1 = msg.buttons[5] == 1 + self.L2 = msg.buttons[6] == 1 + self.R2 = msg.buttons[7] == 1 self.left_analog_x = msg.axes[0] self.left_analog_y = msg.axes[1] self.right_analog_x = msg.axes[5] @@ -410,74 +229,23 @@ class PS4WiredStatus(JoyStatus): def __init__(self, msg): JoyStatus.__init__(self) # creating from sensor_msg/Joy - if msg.buttons[10] == 1: - self.center = True - else: - self.center = False - if msg.buttons[8] == 1: - self.select = True - else: - self.select = False - if msg.buttons[9] == 1: - self.start = True - else: - self.start = False - if msg.buttons[11] == 1: - self.L3 = True - else: - self.L3 = False - if msg.buttons[12] == 1: - self.R3 = True - else: - self.R3 = False - if msg.buttons[3] == 1: - self.square = True - else: - self.square = False - if msg.axes[7] < 0: - self.up = True - else: - self.up = False - if msg.axes[7] > 0: - self.down = True - else: - self.down = False - if msg.axes[6] < 0: - self.left = True - else: - self.left = False - if msg.axes[6] > 0: - self.right = True - else: - self.right = False - if msg.buttons[2] == 1: - self.triangle = True - else: - self.triangle = False - if msg.buttons[0] == 1: - self.cross = True - else: - self.cross = False - if msg.buttons[1] == 1: - self.circle = True - else: - self.circle = False - if msg.buttons[4] == 1: - self.L1 = True - else: - self.L1 = False - if msg.buttons[5] == 1: - self.R1 = True - else: - self.R1 = False - if msg.buttons[6] == 1: - self.L2 = True - else: - self.L2 = False - if msg.buttons[7] == 1: - self.R2 = True - else: - self.R2 = False + self.center = msg.buttons[10] == 1 + self.select = msg.buttons[8] == 1 + self.start = msg.buttons[9] == 1 + self.L3 = msg.buttons[11] == 1 + self.R3 = msg.buttons[12] == 1 + self.square = msg.buttons[3] == 1 + self.up = msg.axes[7] < 0 + self.down = msg.axes[7] > 0 + self.left = msg.axes[6] < 0 + self.right = msg.axes[6] > 0 + self.triangle = msg.buttons[2] == 1 + self.cross = msg.buttons[0] == 1 + self.circle = msg.buttons[1] == 1 + self.L1 = msg.buttons[4] == 1 + self.R1 = msg.buttons[5] == 1 + self.L2 = msg.buttons[6] == 1 + self.R2 = msg.buttons[7] == 1 self.left_analog_x = msg.axes[0] self.left_analog_y = msg.axes[1] self.right_analog_x = msg.axes[3] @@ -666,18 +434,26 @@ def waitForInitialPose(self, next_topic, timeout=None): self.marker_lock.release() def joyCB(self, msg): - if len(msg.axes) == 27 and len(msg.buttons) == 19: + axes_amount = len(msg.axes) + buttons_amount = len(msg.buttons) + if axes_amount == 27 and buttons_amount == 19: status = PS3WiredStatus(msg) - elif len(msg.axes) == 8 and len(msg.buttons) == 11: + elif axes_amount == 8 and buttons_amount == 11: status = XBoxStatus(msg) - elif len(msg.axes) == 20 and len(msg.buttons) == 17: + elif axes_amount == 20 and buttons_amount == 17: status = PS3Status(msg) - elif len(msg.axes) == 14 and len(msg.buttons) == 14: + elif axes_amount == 14 and buttons_amount == 14: status = PS4Status(msg) - elif len(msg.axes) == 8 and len(msg.buttons) == 13: + elif axes_amount == 8 and buttons_amount == 13: status = PS4WiredStatus(msg) + elif axes_amount == 6 and buttons_amount == 17: + status = PS3DualShockStatus(msg) else: - raise Exception("Unknown joystick") + raise Exception( + "Unknown joystick, axes: {}, buttons: {}".format( + axes_amount, buttons_amount + ) + ) self.run(status) self.history.add(status) diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_app_plugins/CMakeLists.txt index bd7b6dbc2e..baa7b8ddd4 100644 --- a/moveit_setup_assistant/moveit_setup_app_plugins/CMakeLists.txt +++ b/moveit_setup_assistant/moveit_setup_app_plugins/CMakeLists.txt @@ -29,6 +29,7 @@ add_library(${PROJECT_NAME} src/perception_config.cpp src/perception.cpp src/perception_widget.cpp + src/xml_manipulation.cpp ${MOC_FILES} ) target_include_directories(${PROJECT_NAME} PUBLIC diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/simulation_widget.hpp b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/simulation_widget.hpp index 8fb5f79206..d6f1e28113 100644 --- a/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/simulation_widget.hpp +++ b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/simulation_widget.hpp @@ -38,6 +38,7 @@ // Qt class QLabel; class QTextEdit; +class QPushButton; // SA #ifndef Q_MOC_RUN @@ -63,18 +64,24 @@ class SimulationWidget : public SetupScreenWidget // ****************************************************************************************** SimulationWidget(QWidget* parent, const MoveItConfigDataPtr& config_data); + void focusGiven() override; + bool focusLost() override; -private Q_SLOTS: +private: + /// Generate Gazebo-compatible URDF, starting from original URDF + std::string generateGazeboCompatibleURDF() const; +private Q_SLOTS: // ****************************************************************************************** // Slot Event Functions // ****************************************************************************************** - // Called the copy to clipboard button is clicked - void copyURDF(const QString& link); - - /// Generate URDF button clicked - void generateURDFClick(); + /// Overwrite original URDF with content of document + void overwriteURDF(); + /// Open original URDF with system editor + void openURDF(); + /// Copy the content of the URDF document to the clipboard + void copyURDF(); private: // ****************************************************************************************** @@ -83,6 +90,8 @@ private Q_SLOTS: QTextEdit* simulation_text_; QLabel* no_changes_label_; + QPushButton* btn_overwrite_; + QPushButton* btn_open_; QLabel* copy_urdf_; /// Contains all the configuration data for the setup assistant diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/xml_manipulation.hpp b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/xml_manipulation.hpp new file mode 100644 index 0000000000..f198865903 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/xml_manipulation.hpp @@ -0,0 +1,63 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Bielefeld University, 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 Bielefeld University 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: Robert Haschke */ + +#pragma once + +// TODO: This header uses tinyxml.h whereas moveit_setup_framework/utilities.hpp uses tinyxml2.h +// One xml library should be selected and used throughout moveit_setup_assistant + +#include +#include + +namespace moveit_setup_app_plugins +{ +struct Attribute +{ + const char* name; + const char* value; + bool required = false; +}; + +/** Insert a new XML element with given tag, attributes and text value + * + * If a corresponding element already exists (and has required attribute values), it is just reused. + * All attributes are created or overwritten with given values. + * A text value is created or overwritten with given value (if not NULL). + * The element is returned */ +TiXmlElement* uniqueInsert(TiXmlElement& element, const char* tag, const std::vector& attributes = {}, + const char* text = nullptr); + +} // namespace moveit_setup_app_plugins diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/src/simulation_widget.cpp b/moveit_setup_assistant/moveit_setup_app_plugins/src/simulation_widget.cpp index bccb4cb4e5..6c8989bc2c 100644 --- a/moveit_setup_assistant/moveit_setup_app_plugins/src/simulation_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_app_plugins/src/simulation_widget.cpp @@ -36,6 +36,8 @@ // SA #include "simulation_widget.h" #include "header_widget.h" +#include +#include // Qt #include @@ -44,6 +46,7 @@ #include #include #include +#include #include #include @@ -70,161 +73,236 @@ SimulationWidget::SimulationWidget(QWidget* parent, const MoveItConfigDataPtr& c "needed changes are shown in green.", this); layout->addWidget(header); + layout->addSpacerItem(new QSpacerItem(1, 8, QSizePolicy::Fixed, QSizePolicy::Fixed)); - // Spacing - QSpacerItem* blank_space = new QSpacerItem(1, 8); - layout->addSpacerItem(blank_space); - - QLabel* instructions = new QLabel(this); - instructions->setText("You can run the following command to quickly find the necessary URDF file to edit:"); - layout->addWidget(instructions); - - QTextEdit* instructions_command = new QTextEdit(this); - instructions_command->setText(std::string("roscd " + config_data->urdf_pkg_name_).c_str()); - instructions_command->setReadOnly(true); - instructions_command->setMaximumHeight(30); - layout->addWidget(instructions_command); - - // Spacing - blank_space = new QSpacerItem(1, 6); - layout->addSpacerItem(blank_space); - - // Used to make the new URDF visible - QPushButton* btn_generate = new QPushButton("&Generate URDF", this); - btn_generate->setMinimumWidth(180); - btn_generate->setMinimumHeight(40); - layout->addWidget(btn_generate); - layout->setAlignment(btn_generate, Qt::AlignLeft); - connect(btn_generate, SIGNAL(clicked()), this, SLOT(generateURDFClick())); - - // When there wa no change to be made + // Top Buttons -------------------------------------------------- + QHBoxLayout* controls_layout = new QHBoxLayout(); + + // Used to overwrite the original URDF + btn_overwrite_ = new QPushButton("Over&write original URDF", this); + btn_overwrite_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); + connect(btn_overwrite_, SIGNAL(clicked()), this, SLOT(overwriteURDF())); + controls_layout->addWidget(btn_overwrite_); + + btn_open_ = new QPushButton("&Open original URDF", this); + btn_open_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); + btn_open_->setToolTip("Open original URDF file in editor"); + connect(btn_open_, SIGNAL(clicked()), this, SLOT(openURDF())); + controls_layout->addWidget(btn_open_); + + // Align buttons to the left + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Fixed)); + + // Add layout + layout->addLayout(controls_layout); + + // When there are no changes to be made no_changes_label_ = new QLabel(this); - no_changes_label_->setText("No Changes To Be Made"); + no_changes_label_->setText("URDF is ready for Gazebo. No changes required."); + no_changes_label_->setFont(QFont(QFont().defaultFamily(), 18)); + no_changes_label_->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); + no_changes_label_->setAlignment(Qt::AlignTop); layout->addWidget(no_changes_label_); - no_changes_label_->setVisible(false); // URDF text simulation_text_ = new QTextEdit(this); simulation_text_->setLineWrapMode(QTextEdit::NoWrap); layout->addWidget(simulation_text_); + // Configure highlighter + auto highlighter = new XmlSyntaxHighlighter(simulation_text_->document()); + QTextCharFormat format; + format.setForeground(Qt::darkGreen); + highlighter->addTag("inertial", format); + highlighter->addTag("transmission", format); + highlighter->addTag("gazebo", format); // Copy URDF link, hidden initially copy_urdf_ = new QLabel(this); copy_urdf_->setText("Copy to Clipboard"); - connect(copy_urdf_, SIGNAL(linkActivated(const QString)), this, SLOT(copyURDF(const QString))); - copy_urdf_->setVisible(false); + connect(copy_urdf_, &QLabel::linkActivated, this, &SimulationWidget::copyURDF); layout->addWidget(copy_urdf_); // Finish Layout -------------------------------------------------- this->setLayout(layout); } -// ****************************************************************************************** -// Called when generate URDF button is clicked -// ****************************************************************************************** -void SimulationWidget::generateURDFClick() +void SimulationWidget::focusGiven() { - simulation_text_->setVisible(true); - std::string gazebo_compatible_urdf_string = config_data_->getGazeboCompatibleURDF(); - std::size_t urdf_length = gazebo_compatible_urdf_string.length(); - - // Check if the urdf do need new elements to be added - if (urdf_length > 0) - { - // Split the added elements from the original urdf to view them in different colors - std::smatch start_match; - std::smatch end_match; - std::regex start_reg_ex(" inertial_opening_matches; - std::vector inertial_closing_matches; - - inertial_closing_matches.push_back(0); - - // Cache the positions of the openings of the inertial elements - for (auto it = std::sregex_iterator(gazebo_compatible_urdf_string.begin(), gazebo_compatible_urdf_string.end(), - start_reg_ex); - it != std::sregex_iterator(); ++it) - { - inertial_opening_matches.push_back(it->position()); - } - - inertial_opening_matches.push_back(urdf_length); + if (!simulation_text_->document()->isEmpty()) + return; // nothing to do - // Cache the positions of the closings of the inertial elements - for (auto it = std::sregex_iterator(gazebo_compatible_urdf_string.begin(), gazebo_compatible_urdf_string.end(), - end_reg_ex); - it != std::sregex_iterator(); ++it) - { - inertial_closing_matches.push_back(it->position()); - } - - for (std::size_t match_number = 0; match_number < inertial_opening_matches.size() - 1; ++match_number) - { - // Show the unmodified elements in black - simulation_text_->setTextColor(QColor("black")); - simulation_text_->append( - QString(gazebo_compatible_urdf_string - .substr(inertial_closing_matches[match_number], - inertial_opening_matches[match_number] - inertial_closing_matches[match_number]) - .c_str())); - - // Show the added elements in green - simulation_text_->setTextColor(QColor("green")); - - simulation_text_->append( - QString(gazebo_compatible_urdf_string - .substr(inertial_opening_matches[match_number], - inertial_closing_matches[match_number + 1] - inertial_opening_matches[match_number] + 11) - .c_str())); - inertial_closing_matches[match_number + 1] += 11; - } - - // Position of the first transmission element in the urdf - std::size_t first_transmission = gazebo_compatible_urdf_string.find("setTextColor(QColor("black")); - simulation_text_->append( - QString(gazebo_compatible_urdf_string.substr(last_inertial, first_transmission - last_inertial).c_str())); - - // Write from the first transmission element until the closing robot element in green - simulation_text_->setTextColor(QColor("green")); - simulation_text_->append(QString( - gazebo_compatible_urdf_string.substr(first_transmission, urdf_length - first_transmission - 10).c_str())); - - // Write the closing robot element in black - simulation_text_->setTextColor(QColor("black")); - simulation_text_->append(QString("")); - } + simulation_text_->setVisible(true); + std::string text = generateGazeboCompatibleURDF(); + config_data_->gazebo_urdf_string_ = text; + + simulation_text_->document()->setPlainText(QString::fromStdString(text)); + + // Add generated Gazebo URDF to config file if not empty + bool have_changes = !text.empty(); + config_data_->save_gazebo_urdf_ = have_changes; + + // GUI elements are visible only if there are URDF changes to display/edit + simulation_text_->setVisible(have_changes); + btn_overwrite_->setVisible(have_changes); + btn_open_->setVisible(have_changes && !qgetenv("EDITOR").isEmpty()); + copy_urdf_->setVisible(have_changes); + no_changes_label_->setVisible(!have_changes); + + // Disable overwrite button if URDF originates from xacro + btn_overwrite_->setDisabled(config_data_->urdf_from_xacro_); + QString tooltip; + if (btn_overwrite_->isEnabled()) + tooltip = tr("Overwrite URDF in original location:\n").append(config_data_->urdf_path_.c_str()); + else + tooltip = tr("Cannot overwrite original, xacro-based URDF"); + btn_overwrite_->setToolTip(tooltip); - // Copy link appears after the text is ready - copy_urdf_->setVisible(true); - } + if (have_changes) + config_data_->changes |= MoveItConfigData::SIMULATION; else + config_data_->changes &= ~MoveItConfigData::SIMULATION; +} + +bool SimulationWidget::focusLost() +{ + if (!config_data_->save_gazebo_urdf_) + return true; // saving is disabled anyway + + // validate XML + TiXmlDocument doc; + auto urdf = simulation_text_->document()->toPlainText().toStdString(); + doc.Parse(urdf.c_str(), nullptr, TIXML_ENCODING_UTF8); + if (doc.Error()) { - simulation_text_->append(QString(gazebo_compatible_urdf_string.c_str())); - no_changes_label_->setVisible(true); + QTextCursor cursor = simulation_text_->textCursor(); + cursor.movePosition(QTextCursor::Start); + cursor.movePosition(QTextCursor::Down, QTextCursor::MoveAnchor, doc.ErrorRow()); + cursor.movePosition(QTextCursor::Right, QTextCursor::MoveAnchor, doc.ErrorCol()); + simulation_text_->setTextCursor(cursor); + QMessageBox::warning(this, tr("Gazebo URDF"), tr("Error parsing XML:\n").append(doc.ErrorDesc())); + simulation_text_->setFocus(Qt::OtherFocusReason); + return false; // reject switching } + else + config_data_->gazebo_urdf_string_ = std::move(urdf); + return true; +} + +// ****************************************************************************************** +// Called when save URDF button is clicked +// ****************************************************************************************** +void SimulationWidget::overwriteURDF() +{ + if (!focusLost()) // validate XML + return; + + if (!config_data_->outputGazeboURDFFile(config_data_->urdf_path_)) + QMessageBox::warning(this, "Gazebo URDF", tr("Failed to save to ").append(config_data_->urdf_path_.c_str())); + else // Display success message + QMessageBox::information(this, "Overwriting Successful", + "Original robot description URDF was successfully overwritten."); + + // Remove Gazebo URDF file from list of to-be-written config files + config_data_->save_gazebo_urdf_ = false; + config_data_->changes &= ~MoveItConfigData::SIMULATION; +} + +void SimulationWidget::openURDF() +{ + QProcess::startDetached(qgetenv("EDITOR"), { config_data_->urdf_path_.c_str() }); } // ****************************************************************************************** // Called the copy to clipboard button is clicked // ****************************************************************************************** -void SimulationWidget::copyURDF(const QString& /*link*/) +void SimulationWidget::copyURDF() { simulation_text_->selectAll(); simulation_text_->copy(); } +// Generate a Gazebo-compatible robot URDF +std::string SimulationWidget::generateGazeboCompatibleURDF() const +{ + TiXmlDocument doc; + doc.Parse(config_data_->urdf_string_.c_str(), nullptr, TIXML_ENCODING_UTF8); + auto root = doc.RootElement(); + + // Normalize original urdf_string_ + TiXmlPrinter orig_urdf; + doc.Accept(&orig_urdf); + + // Map existing SimpleTransmission elements to their joint name + std::map transmission_elements; + for (TiXmlElement* element = root->FirstChildElement("transmission"); element != nullptr; + element = element->NextSiblingElement(element->Value())) + { + auto type_tag = element->FirstChildElement("type"); + auto joint_tag = element->FirstChildElement("joint"); + if (!type_tag || !type_tag->GetText() || !joint_tag || !joint_tag->Attribute("name")) + continue; // ignore invalid tags + if (std::string(type_tag->GetText()) == "transmission_interface/SimpleTransmission") + transmission_elements[element->FirstChildElement("joint")->Attribute("name")] = element; + } + + // Loop through Link and Joint elements and add Gazebo tags if not present + for (TiXmlElement* element = root->FirstChildElement(); element != nullptr; element = element->NextSiblingElement()) + { + const std::string tag_name(element->Value()); + if (tag_name == "link" && element->FirstChildElement("collision")) + { + TiXmlElement* inertial = uniqueInsert(*element, "inertial"); + uniqueInsert(*inertial, "mass", { { "value", "0.1" } }); + uniqueInsert(*inertial, "origin", { { "xyz", "0 0 0" }, { "rpy", "0 0 0" } }); + uniqueInsert(*inertial, "inertia", + { { "ixx", "0.03" }, + { "iyy", "0.03" }, + { "izz", "0.03" }, + { "ixy", "0.0" }, + { "ixz", "0.0" }, + { "iyz", "0.0" } }); + } + else if (tag_name == "joint") + { + const char* joint_type = element->Attribute("type"); + const char* joint_name = element->Attribute("name"); + if (!joint_type || !joint_name || strcmp(joint_type, "fixed") == 0) + continue; // skip invalid or fixed joints + + // find existing or create new transmission element for this joint + TiXmlElement* transmission; + auto it = transmission_elements.find(joint_name); + if (it != transmission_elements.end()) + transmission = it->second; + else + { + transmission = root->InsertEndChild(TiXmlElement("transmission"))->ToElement(); + transmission->SetAttribute("name", std::string("trans_") + joint_name); + } + + uniqueInsert(*transmission, "type", {}, "transmission_interface/SimpleTransmission"); + + std::string hw_interface = config_data_->getJointHardwareInterface(joint_name); + auto* joint = uniqueInsert(*transmission, "joint", { { "name", joint_name } }); + uniqueInsert(*joint, "hardwareInterface", {}, hw_interface.c_str()); + + auto actuator_name = joint_name + std::string("_motor"); + auto* actuator = uniqueInsert(*transmission, "actuator", { { "name", actuator_name.c_str() } }); + uniqueInsert(*actuator, "hardwareInterface", {}, hw_interface.c_str()); + uniqueInsert(*actuator, "mechanicalReduction", {}, "1"); + } + } + + // Add gazebo_ros_control plugin which reads the transmission tags + TiXmlElement* gazebo = uniqueInsert(*root, "gazebo"); + TiXmlElement* plugin = uniqueInsert(*gazebo, "plugin", { { "name", "gazebo_ros_control", true } }); + uniqueInsert(*plugin, "robotNamespace", {}, "/"); + + // generate new URDF + TiXmlPrinter new_urdf; + doc.Accept(&new_urdf); + // and return it when there are changes + return orig_urdf.Str() == new_urdf.Str() ? std::string() : new_urdf.Str(); +} + } // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/src/xml_manipulation.cpp b/moveit_setup_assistant/moveit_setup_app_plugins/src/xml_manipulation.cpp new file mode 100644 index 0000000000..dad27a8925 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/src/xml_manipulation.cpp @@ -0,0 +1,84 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Bielefeld University, 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 Bielefeld University 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: Robert Haschke */ + +#include + +namespace moveit_setup_app_plugins +{ +namespace +{ +bool hasRequiredAttributes(const TiXmlElement& e, const std::vector& attributes) +{ + for (const auto& attr : attributes) + { + if (!attr.required) + continue; // attribute not required + const char* value = e.Attribute(attr.name); + if (value && strcmp(attr.value, value) == 0) + continue; // attribute has required value + else + return false; + } + return true; +}; +} // namespace + +TiXmlElement* uniqueInsert(TiXmlElement& element, const char* tag, const std::vector& attributes, + const char* text) +{ + // search for existing element with required tag name and attributes + TiXmlElement* result = element.FirstChildElement(tag); + while (result && !hasRequiredAttributes(*result, attributes)) + result = result->NextSiblingElement(tag); + + if (!result) // if not yet present, create new element + result = element.InsertEndChild(TiXmlElement(tag))->ToElement(); + + // set (not-yet existing) attributes + for (const auto& attr : attributes) + { + if (!result->Attribute(attr.name)) + result->SetAttribute(attr.name, attr.value); + } + + // insert text if required + if (text && !result->GetText()) + result->InsertEndChild(TiXmlText(text)); + + return result; +} + +} // namespace moveit_setup_app_plugins diff --git a/moveit_setup_assistant/moveit_setup_framework/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_framework/CMakeLists.txt index c7503616b4..1e876e2bc5 100644 --- a/moveit_setup_assistant/moveit_setup_framework/CMakeLists.txt +++ b/moveit_setup_assistant/moveit_setup_framework/CMakeLists.txt @@ -38,6 +38,7 @@ add_library(${PROJECT_NAME} src/rviz_panel.cpp src/data_warehouse.cpp src/templates.cpp + src/xml_syntax_highlighter.cpp ${MOC_FILES} ) target_include_directories(${PROJECT_NAME} PUBLIC diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/package_settings_config.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/package_settings_config.hpp index f51254e384..2153a1eb3b 100644 --- a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/package_settings_config.hpp +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/package_settings_config.hpp @@ -39,7 +39,7 @@ namespace moveit_setup_framework { -const std::string SETUP_ASSISTANT_FILE = ".setup_assistant"; +static const std::string SETUP_ASSISTANT_FILE = ".setup_assistant"; class PackageSettingsConfig : public SetupConfig { diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/urdf_config.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/urdf_config.hpp index cfcb234871..6ba18129fc 100644 --- a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/urdf_config.hpp +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/urdf_config.hpp @@ -68,6 +68,16 @@ class URDFConfig : public SetupConfig return urdf_model_; } + std::string getURDFPackageName() const + { + return urdf_pkg_name_; + } + + std::string getURDFContents() const + { + return urdf_string_; + } + std::string getURDFPath() const { return urdf_path_; diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/xml_syntax_highlighter.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/xml_syntax_highlighter.hpp new file mode 100644 index 0000000000..48149e15a5 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/xml_syntax_highlighter.hpp @@ -0,0 +1,69 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Bielefeld University, 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 Bielefeld University 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: Robert Haschke */ + +#pragma once + +#include +#include +#include + +namespace moveit_setup_framework +{ +/** XML SyntaxHighlighter allowing nested highlighting of XML tags */ +class XmlSyntaxHighlighter : public QSyntaxHighlighter +{ +public: + XmlSyntaxHighlighter(QTextDocument* parent = nullptr); + void addTag(const QString& tag, const QTextCharFormat& format, const QString& parent = QString()); + +protected: + void highlightBlock(const QString& text) override; + +private: + struct Rule + { + QRegularExpression start; + QRegularExpression end; + QTextCharFormat format; + std::map::const_iterator parent; + }; + using Rules = std::map; + Rules rules; + + Rules::const_iterator highlight(Rules::const_iterator active, QStringRef text, int start, bool search_end, int& end); +}; + +} // namespace moveit_setup_framework diff --git a/moveit_setup_assistant/moveit_setup_framework/src/helper_widgets.cpp b/moveit_setup_assistant/moveit_setup_framework/src/helper_widgets.cpp index 97e9a5bb33..14c3a6de38 100644 --- a/moveit_setup_assistant/moveit_setup_framework/src/helper_widgets.cpp +++ b/moveit_setup_assistant/moveit_setup_framework/src/helper_widgets.cpp @@ -54,7 +54,6 @@ HeaderWidget::HeaderWidget(const std::string& title, const std::string& instruct { // Basic widget container QVBoxLayout* layout = new QVBoxLayout(this); - layout->setAlignment(Qt::AlignTop); // Page Title QLabel* page_title = new QLabel(this); @@ -62,23 +61,23 @@ HeaderWidget::HeaderWidget(const std::string& title, const std::string& instruct QFont page_title_font(QFont().defaultFamily(), 18, QFont::Bold); page_title->setFont(page_title_font); page_title->setWordWrap(true); + page_title->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Maximum); layout->addWidget(page_title); - layout->setAlignment(page_title, Qt::AlignTop); // Page Instructions QLabel* page_instructions = new QLabel(this); page_instructions->setText(instructions.c_str()); page_instructions->setWordWrap(true); - // page_instructions->setSizePolicy( QSizePolicy::Preferred, QSizePolicy::Expanding ); - page_instructions->setMinimumWidth(1); + page_instructions->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Maximum); layout->addWidget(page_instructions); - layout->setAlignment(page_instructions, Qt::AlignTop); // Margin on bottom layout->setContentsMargins(0, 0, 0, 0); // last 15 this->setLayout(layout); - // this->setSizePolicy( QSizePolicy::Preferred, QSizePolicy::Expanding ); + + // For some reason, this style sheet setting affects the placement layout!? + this->setStyleSheet(QString("background-color:%1;").arg(QWidget::palette().color(QWidget::backgroundRole()).name())); } // ****************************************************************************************** diff --git a/moveit_setup_assistant/moveit_setup_framework/src/xml_syntax_highlighter.cpp b/moveit_setup_assistant/moveit_setup_framework/src/xml_syntax_highlighter.cpp new file mode 100644 index 0000000000..a9adc7f300 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/src/xml_syntax_highlighter.cpp @@ -0,0 +1,128 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Bielefeld University, 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 Bielefeld University 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: Robert Haschke */ + +#include +#include + +namespace moveit_setup_framework +{ +XmlSyntaxHighlighter::XmlSyntaxHighlighter(QTextDocument* parent) : QSyntaxHighlighter(parent) +{ +} + +void XmlSyntaxHighlighter::addTag(const QString& tag, const QTextCharFormat& format, const QString& parent) +{ + const QString start_pattern("<%1.*?/?>"); + Rule rule; + rule.start = QRegularExpression(start_pattern.arg(tag)); + rule.end = QRegularExpression(QString("|<%1[^>]*?/>").arg(tag)); + rule.format = format; + if (!parent.isEmpty()) + { + QString parent_start = start_pattern.arg(parent); + rule.parent = std::find_if(rules.begin(), rules.end(), [&](const std::pair& rule) { + return rule.second.start.pattern() == parent_start; + }); + } + else + rule.parent = rules.end(); + + rules.insert(std::make_pair(rules.size(), rule)); +} + +XmlSyntaxHighlighter::Rules::const_iterator +XmlSyntaxHighlighter::highlight(Rules::const_iterator active, QStringRef text, int start, bool search_end, int& end) +{ + int offset = end; // when passed, end indicates the end of the opening expression + auto next = active; // return value: active rule at end of text + + if (search_end) // find end of active rule + { + auto match = active->second.end.match(text); + // when returned, end indicates the end of the closing expression + end = match.hasMatch() ? match.capturedEnd() : text.size(); + setFormat(start, end, active->second.format); + if (match.hasMatch()) + { + text = text.left(match.capturedStart()); // drop text after (and including) closing expression + next = active->second.parent; + } + } + text = text.mid(offset); // skip opening expression + start += offset; // adjust start by skipped offset + if (text.isEmpty()) + return next; // early return + + // highlight remaining text using active's children's rules + for (auto it = rules.begin(); it != rules.end(); ++it) + { + const auto& rule = it->second; + if (rule.parent != active) + continue; // skip wrong rules + + offset = 0; // (re)start at beginning of (clipped) text + while (true) // process all matches of rule + { + auto match = rule.start.match(text, offset); + if (!match.hasMatch()) + break; + + offset = match.capturedEnd() - match.capturedStart(); // mark end of opening expression in passed text + auto result = highlight(it, text.mid(match.capturedStart()), start + match.capturedStart(), true, offset); + // returned offset is w.r.t. beginning of _passed_ text: add passed start offset to yield offset w.r.t. text + offset += match.capturedStart(); + if (result == it) // text is ending with this rule + { + assert(next == active || next == active->second.parent); + assert(offset == text.size()); // end should mark the end of the text + next = result; // remember return value: active rule at end of text + break; + } + } + } + + return next; +} + +void XmlSyntaxHighlighter::highlightBlock(const QString& text) +{ + Rules::const_iterator active = previousBlockState() < 0 ? rules.end() : rules.find(previousBlockState()); + int unused = 0; + active = highlight(active, QStringRef(&text, 0, text.size()), 0, active != rules.cend(), unused); + setCurrentBlockState(active != rules.cend() ? active->first : -1); +} + +} // namespace moveit_setup_framework diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/default_collisions.hpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/default_collisions.hpp index 50d6402113..f618dd76e9 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/default_collisions.hpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/default_collisions.hpp @@ -44,7 +44,8 @@ struct CollisionPairLess { bool operator()(const srdf::Model::CollisionPair& left, const srdf::Model::CollisionPair& right) const { - return left.link1_ < right.link1_ && left.link2_ < right.link2_; + // use std::pair's operator<: (left.link1_, left.link2_) < (right.link1_, right.link2_) + return left.link1_ < right.link1_ || (!(right.link1_ < left.link1_) && left.link2_ < right.link2_); } }; diff --git a/moveit_setup_assistant/old_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/old_assistant/src/tools/moveit_config_data.cpp index ffc50a81b6..2105d4df26 100644 --- a/moveit_setup_assistant/old_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/old_assistant/src/tools/moveit_config_data.cpp @@ -372,112 +372,6 @@ std::string MoveItConfigData::getJointHardwareInterface(const std::string& joint return "hardware_interface/EffortJointInterface"; } -// ****************************************************************************************** -// Writes a Gazebo compatible robot URDF to gazebo_compatible_urdf_string_ -// ****************************************************************************************** -std::string MoveItConfigData::getGazeboCompatibleURDF() -{ - bool new_urdf_needed = false; - TiXmlDocument urdf_document; - - // Used to convert XmlDocument to std string - TiXmlPrinter printer; - urdf_document.Parse((const char*)urdf_string_.c_str(), nullptr, TIXML_ENCODING_UTF8); - try - { - for (TiXmlElement* doc_element = urdf_document.RootElement()->FirstChildElement(); doc_element != nullptr; - doc_element = doc_element->NextSiblingElement()) - { - if (static_cast(doc_element->Value()).find("link") != std::string::npos) - { - // Before adding inertial elements, make sure there is none and the link has collision element - if (doc_element->FirstChildElement("inertial") == nullptr && - doc_element->FirstChildElement("collision") != nullptr) - { - new_urdf_needed = true; - TiXmlElement inertia_link("inertial"); - TiXmlElement mass("mass"); - TiXmlElement inertia_joint("inertia"); - - mass.SetAttribute("value", "0.1"); - - inertia_joint.SetAttribute("ixx", "0.03"); - inertia_joint.SetAttribute("iyy", "0.03"); - inertia_joint.SetAttribute("izz", "0.03"); - inertia_joint.SetAttribute("ixy", "0.0"); - inertia_joint.SetAttribute("ixz", "0.0"); - inertia_joint.SetAttribute("iyz", "0.0"); - - inertia_link.InsertEndChild(mass); - inertia_link.InsertEndChild(inertia_joint); - - doc_element->InsertEndChild(inertia_link); - } - } - else if (static_cast(doc_element->Value()).find("joint") != std::string::npos) - { - // Before adding a transmission element, make sure there the joint is not fixed - if (static_cast(doc_element->Attribute("type")) != "fixed") - { - new_urdf_needed = true; - std::string joint_name = static_cast(doc_element->Attribute("name")); - TiXmlElement transmission("transmission"); - TiXmlElement type("type"); - TiXmlElement joint("joint"); - TiXmlElement hardware_interface("hardwareInterface"); - TiXmlElement actuator("actuator"); - TiXmlElement mechanical_reduction("mechanicalReduction"); - - transmission.SetAttribute("name", std::string("trans_") + joint_name); - joint.SetAttribute("name", joint_name); - actuator.SetAttribute("name", joint_name + std::string("_motor")); - - type.InsertEndChild(TiXmlText("transmission_interface/SimpleTransmission")); - transmission.InsertEndChild(type); - - hardware_interface.InsertEndChild(TiXmlText(getJointHardwareInterface(joint_name).c_str())); - joint.InsertEndChild(hardware_interface); - transmission.InsertEndChild(joint); - - mechanical_reduction.InsertEndChild(TiXmlText("1")); - actuator.InsertEndChild(hardware_interface); - actuator.InsertEndChild(mechanical_reduction); - transmission.InsertEndChild(actuator); - - urdf_document.RootElement()->InsertEndChild(transmission); - } - } - } - - // Add gazebo_ros_control plugin which reads the transmission tags - TiXmlElement gazebo("gazebo"); - TiXmlElement plugin("plugin"); - TiXmlElement robot_namespace("robotNamespace"); - - plugin.SetAttribute("name", "gazebo_ros_control"); - plugin.SetAttribute("filename", "libgazebo_ros_control.so"); - robot_namespace.InsertEndChild(TiXmlText(std::string("/"))); - - plugin.InsertEndChild(robot_namespace); - gazebo.InsertEndChild(plugin); - - urdf_document.RootElement()->InsertEndChild(gazebo); - } - catch (YAML::ParserException& e) // Catch errors - { - RCLCPP_ERROR_STREAM(LOGGER, e.what()); - return std::string(""); - } - - if (new_urdf_needed) - { - urdf_document.Accept(&printer); - return std::string(printer.CStr()); - } - - return std::string(""); -} - bool MoveItConfigData::outputFakeControllersYAML(const std::string& file_path) { YAML::Emitter emitter;