Custom Class with Move Group and Publisher

asked 2022-05-19 14:39:26 -0500

david.mark gravatar image

I am trying to create a class that can get the end effector pose using the move group API while also publishing and subscribing to a few topics. I am running ROS2 Galactic. This is the code:

class Admitt_Servo : public moveit::planning_interface::MoveGroupInterface{
 public:
  Admitt_Servo(rclcpp::Node::SharedPtr node) : moveit::planning_interface::MoveGroupInterface(node, "ur_manipulator")
  {
      sub_fts_ = node->create_subscription<geometry_msgs::msg::WrenchStamped>("/ft_data", 1, [this](geometry_msgs::msg::WrenchStamped::SharedPtr msg_ft) { UpdateWrenchCB(msg_ft); });
      sub_april_pose_ = node->create_subscription<tf2_msgs::msg::TFMessage>("/apriltag/tf", 1, [this](tf2_msgs::msg::TFMessage::SharedPtr msg_pose) { DesiredPoseCB(msg_pose); });
      pub_servo_ = node->create_publisher<geometry_msgs::msg::TwistStamped>("/servo_node/delta_twist_cmds",10);

      curr_pose_ = this->getCurrentPose();
   }

  // Other Member Functions and variables
};

int main(int argc, char * argv[]){
  rclcpp::init(argc,argv);
  rclcpp::NodeOptions node_options;
  node_options.automatically_declare_parameters_from_overrides(true);
  auto add_srv_node = rclcpp::Node::make_shared("add_srv_node", node_options);

  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(add_srv_node);
  std::thread([&executor]() { executor.spin(); }).detach();

  Admitt_Servo admitt_servo(add_srv_node);

  rclcpp::shutdown();
  return 0;
}

When I launch the node I get this error:

[INFO] [launch]: All log files can be found below /home/ngrobots/.ros/log/2022-05-19-15-13-03-748519-ngrobots-dev-176650
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [add_srv_node-1]: process started with pid [176654]
[add_srv_node-1] [WARN] [1652987584.276172018] [rcl.logging_rosout]: Publisher already registered for provided node name. 
If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As 
soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from 
being published on the rosout topic.
[add_srv_node-1] [INFO] [1652987584.286040015] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.00441764 seconds
[add_srv_node-1] [INFO] [1652987584.286087921] [moveit_robot_model.robot_model]: Loading robot model 'ur'...
[add_srv_node-1] [INFO] [1652987584.286098551] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. 
Assuming fixed joint
[add_srv_node-1] Link robot_cart_dummy_base had 1 children
[add_srv_node-1] Link stand had 1 children
[add_srv_node-1] Link platform had 1 children
[add_srv_node-1] Link base_link had 2 children
[add_srv_node-1] Link base had 0 children
[add_srv_node-1] Link base_link_inertia had 1 children
[add_srv_node-1] Link shoulder_link had 1 children
[add_srv_node-1] Link upper_arm_link had 1 children
[add_srv_node-1] Link forearm_link had 1 children
[add_srv_node-1] Link wrist_1_link had 1 children
[add_srv_node-1] Link wrist_2_link had 1 children
[add_srv_node-1] Link wrist_3_link had 2 children
[add_srv_node-1] Link flange had 1 children
[add_srv_node-1] Link tool0 had 1 children
[add_srv_node-1] Link camera_bottom_screw_frame had 3 children
[add_srv_node-1] Link camera_link had 3 children
[add_srv_node-1] Link camera_color_frame had 3 children
[add_srv_node-1] Link camera_accel_frame had 1 children
[add_srv_node-1] Link camera_accel_optical_frame had 0 children
[add_srv_node-1] Link camera_color_optical_frame had 0 children
[add_srv_node-1] Link camera_gyro_frame had 1 children
[add_srv_node-1] Link camera_gyro_optical_frame had 0 children
[add_srv_node-1] Link camera_depth_frame had 1 children
[add_srv_node-1] Link camera_depth_optical_frame had 0 children
[add_srv_node-1] Link camera_infra_frame had 1 children
[add_srv_node-1] Link camera_infra_optical_frame had 0 children
[add_srv_node-1] Link camera_lower_mount had 0 children
[add_srv_node-1] Link camera_upper_mount had 0 children
[add_srv_node-1] Link ft_frame had 0 children
[add_srv_node-1] [INFO] [1652987584.316381238] [move_group_interface]: Ready to take commands for planning group ur_manipulator.
[add_srv_node-1] [INFO] [1652987584.317925109] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states'
[add_srv_node-1] [INFO] [1652987585.317996314] [moveit_ros.current_state_monitor]: Didn't received robot state (joint angles) with recent ...
(more)
edit retag flag offensive close merge delete