[MoveIt2] Stuck creating moveit::planning_interface::MoveGroupInterface object
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::string>(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 ...
I don't know, but you might try a MultiThreadedExecutor instead of SingleThreadedExecutor
Thanks for the reply! Tried with MultiThreadedExecutor, but still having the same issue.