0
$\begingroup$

Rosanswers logo

Platform: Ubuntu 22.04
ROS Distro: Humble
Hi,
I am following the MoveIt2 tutorial and created a simple URDF for testing with move_group C++ interface.
I have generated the config files using moveit_setup_assistant.

I have create a repository with my package - link ###Code:


#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/msg/display_robot_state.hpp>
#include <moveit_msgs/msg/display_trajectory.hpp>

#include <moveit_msgs/msg/attached_collision_object.hpp>
#include <moveit_msgs/msg/collision_object.hpp>

#include <moveit_visual_tools/moveit_visual_tools.h>

static const rclcpp::Logger LOGGER = rclcpp::get_logger("move_group_test");
int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions node_options;
  node_options.automatically_declare_parameters_from_overrides(true);
  auto move_group_node = rclcpp::Node::make_shared("move_group_interface_test", node_options);
  RCLCPP_INFO(LOGGER, "MOVE_GROUP CREATED.");

  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(move_group_node);
  std::thread([&executor]() { executor.spin(); }).detach();
  RCLCPP_INFO(LOGGER, "EXECUTOR Added to thread");

  static const std::string PLANNING_GROUP = "joint";
  moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP);
  RCLCPP_INFO(LOGGER, "PLANNING GROUP CREATED");
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
  const moveit::core::JointModelGroup* joint_model_group =
    move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP
  );

  namespace rvt = rviz_visual_tools;
  moveit_visual_tools::MoveItVisualTools visual_tools(
    move_group_node,
    "base_link",
    "/test_topic",
    move_group.getRobotModel()
  );
  visual_tools.deleteAllMarkers();
  visual_tools.loadRemoteControl();
  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
  text_pose.translation().z() = 1.0;
  visual_tools.publishText(text_pose, "MoveGroupInterface_TEST", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  // We can print the name of the reference frame for this robot.
  RCLCPP_INFO(LOGGER, "Planning frame: %s", move_group.getPlanningFrame().c_str());
  // We can also print the name of the end-effector link for this group.
  RCLCPP_INFO(LOGGER, "End effector link: %s", move_group.getEndEffectorLink().c_str());

  // We can get a list of all the groups in the robot:
  RCLCPP_INFO(LOGGER, "Available Planning Groups:");
  std::copy(
    move_group.getJointModelGroupNames().begin(),
    move_group.getJointModelGroupNames().end(),
    std::ostream_iterator(std::cout, ", ")
  );
}

###URDF


<?xml version="1.0"?>
<robot name="single_joint">
  <material name="blue">
    <color rgba="0 0 0.8 1"/>
  </material>
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </collision>
  </link>
  <link name="rotate_joint">
    <visual>
      <geometry>
        <box size="0.6 0.1 0.2"/>
      </geometry>
      <origin rpy="1.57075 0 0" xyz="0.3 0 0"/>
    </visual>
    <collision>
      <geometry>
        <box size="0.6 0.1 0.2"/>
      </geometry>
    </collision>
  </link>
  <joint name="base_to_rotate_joint" type="continuous">
    <parent link="base_link"/>
    <child link="rotate_joint"/>
    <origin xyz="0 0 0.35"/>
    <axis xyz="0 0 1"/>
  </joint>
</robot>

###SRDF


<?xml version="1.0" encoding="UTF-8"?>
<!--This does not replace URDF, and is not an extension of URDF.
    This is a format for representing semantic information about the robot structure.
    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="single_joint">
    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
    <group name="joint">
        <joint name="virtual_joint"/>
        <joint name="base_to_rotate_joint"/>
    </group>
    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
    <group_state name="one" group="joint">
        <joint name="base_to_rotate_joint" value="-2.3778"/>
    </group_state>
    <group_state name="two" group="joint">
        <joint name="base_to_rotate_joint" value="-0.8505"/>
    </group_state>
    <group_state name="three" group="joint">
        <joint name="base_to_rotate_joint" value="0.4686"/>
    </group_state>
    <group_state name="four" group="joint">
        <joint name="base_to_rotate_joint" value="2.1695"/>
    </group_state>
    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
    <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="base_link"/>
</robot>

The program is stuck creating the moveit::planning_interface::MoveGroupInterface object, when the node is starting using the following launch file. I am running a separate joint_state_publisher using "ros2 run". I am able to visualize URDF well in RViz. ###Launch


import os
import yaml
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command

def load_file(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        with open(absolute_file_path, "r") as file:
            return file.read()
    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
        return None

def load_yaml(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        with open(absolute_file_path, "r") as file:
            return yaml.safe_load(file)
    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
        return None

def generate_launch_description():
  robot_description_config = load_file(
    "moveit_test_pkg", "urdf/one_joint.urdf"
  )

  robot_description_semantic_config = load_file(
    "moveit_test_pkg", "config/single_joint.srdf"
  )

  kinematics_yaml = load_yaml(
    "moveit_test_pkg", "config/kinematics.yaml"
  )

  move_group_node = Node(package='moveit_test_pkg', executable='move_group_test',
    output='screen',
    parameters=[{
      'robot_description_semantic': robot_description_semantic_config,
      "robot_description_kinematics": kinematics_yaml,
    }],
  )

  robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="both",
        parameters=[{
          'robot_description': robot_description_config,
          'publish_frequency': 15.0
        }]
    )

  return LaunchDescription([
        robot_state_publisher,
        move_group_node
    ])

###Output


cryoschrome@ROG:~/test_ws$ ros2 launch moveit_test_pkg new_test.launch.py 
[INFO] [launch]: All log files can be found below /home/sourabh/.ros/log/2022-05-30-18-26-13-825026-ROG-101360
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [robot_state_publisher-1]: process started with pid [101361]
[INFO] [move_group_test-2]: process started with pid [101363]
[robot_state_publisher-1] Link rotate_joint had 0 children
[robot_state_publisher-1] [INFO] [1653920774.026640679] [robot_state_publisher]: got segment base_link
[robot_state_publisher-1] [INFO] [1653920774.026770935] [robot_state_publisher]: got segment rotate_joint
[move_group_test-2] [INFO] [1653920774.045723067] [move_group_test]: MOVE_GROUP CREATED.
[move_group_test-2] [INFO] [1653920774.045972264] [move_group_test]: EXECUTOR Added to thread
[move_group_test-2] [INFO] [1653920774.077623685] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0257344 seconds
[move_group_test-2] [INFO] [1653920774.077674670] [moveit_robot_model.robot_model]: Loading robot model 'single_joint'...
[move_group_test-2] Link rotate_joint had 0 children

Any help is appreciated! Thank you.


Originally posted by Cryoschrome on ROS Answers with karma: 75 on 2022-05-30

Post score: 0


Original comments

Comment by AndyZe on 2022-05-31:
I don't know, but you might try a MultiThreadedExecutor instead of SingleThreadedExecutor

Comment by Cryoschrome on 2022-06-01:
Thanks for the reply! Tried with MultiThreadedExecutor, but still having the same issue.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

I did not launch the "move_group" executable from "moveit_ros_move_group" package and hence ran into the issue. The issue was solved by creating a separate launch file, similar to this to launch the above node.


Originally posted by Cryoschrome with karma: 75 on 2022-06-06

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.