Skip to content

Not subscribing to /joint_states (Humble) #1054

@ghost

Description

OS: Ubuntu 22.04 LTS
ROS distro: Humble
Binary/Source: Both ROS2 and MoveIt were built from source.

Description of bug:
I am currently using a UR10e robot in my setup. i have manged to create a WS where i have a custom URDF config and a moveit config package obtained with the setup assistant.
When i open up RViz everything works fine and i can plan and execute.

I tried this tutorial:
https://moveit.picknik.ai/humble/doc/tutorials/your_first_project/your_first_project.html#plan-and-execute-using-movegroupinterface

Since i need to create a node that will expose actions to move the robot to any pose. However this does not seem to work. The robot moves when i send a pose command, and it actually goes to the intended pose, but it never uses the most appropriate path.
Looking aroung on the internet i think the main problem is that the node does not subscribe to the /joint_states topic, meaning that the plan doesn't get computed properly, and the robot goes crazy.

this is a snippet of my Node:

main:

#include "freddy/freddy.hpp"

int main(int argc, char **argv)
{
  // Initialise ROS2 network
  rclcpp::init(argc, argv);
  
  // Declare FreddyNode as shared_ptr with options
  rclcpp::NodeOptions options = rclcpp::NodeOptions();
  options.automatically_declare_parameters_from_overrides(true);
  std::shared_ptr<FreddyNode> node = std::make_shared<FreddyNode>(options);

  // Create the moveit interface from outside the class
  // i can't call this->shared_from_this() inside the constructor of a class
  auto interface = 
    std::make_shared<moveit::planning_interface::MoveGroupInterface>(node, "ur_arm");

  if (!node->setMoveGroupInterface(interface)) {
    rclcpp::shutdown();
    return 1;
  }
  
  // Spinning the node
  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

hpp:

#pragma once

#include <memory>

#include <rclcpp/rclcpp.hpp>

#include <std_srvs/srv/trigger.hpp>
#include <geometry_msgs/msg/pose.hpp>

// MoveIt
#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>


class FreddyNode : public rclcpp::Node {
  public:
    FreddyNode(rclcpp::NodeOptions options);
    // ~FreddyNode() override;

    bool setMoveGroupInterface(const std::shared_ptr<moveit::planning_interface::MoveGroupInterface>& interface);
    
  private:
    // Srv callbacks
    void move_home_callback(const std::shared_ptr<std_srvs::srv::Trigger::Request>  request,
      std::shared_ptr<std_srvs::srv::Trigger::Response> response);
    
    // Class functions
    bool plan_and_execute_pose(const geometry_msgs::msg::Pose& target_pose);

    // // MoveIt pipeline members
    // robot_model_loader::RobotModelLoaderPtr robot_model_loader_;
    // planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
    // planning_pipeline::PlanningPipelinePtr planning_pipeline_;

    // Srv objects
    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr move_home_srv_;

    // Moveit interface
    std::shared_ptr<moveit::planning_interface::MoveGroupInterface> p_move_group_interface;
};

cpp:


#include "freddy/freddy.hpp"

FreddyNode::FreddyNode(rclcpp::NodeOptions options) : Node("freddy_node", options) {
  
  RCLCPP_INFO(this->get_logger(), "Setting up Freddy Node & MoveIt pipeline...");
  
  this->move_home_srv_ = this->create_service<std_srvs::srv::Trigger>(
    "move_home",
    std::bind(&FreddyNode::move_home_callback, this, std::placeholders::_1, std::placeholders::_2)
  );
  RCLCPP_INFO(this->get_logger(), "FreddyNode ready.");
}


//////////////////////////////////
/////// CALLBACK FUNCTIONS ///////
//////////////////////////////////
void FreddyNode::move_home_callback(const std::shared_ptr<std_srvs::srv::Trigger::Request>,
                                          std::shared_ptr<std_srvs::srv::Trigger::Response> response){
  
  // 1. Prepare a pose goal (replace with your robot's home pose and frame)
  // convert from eul to quaternions
  geometry_msgs::msg::Pose home_pose;
  home_pose.position.x = 0.5;
  home_pose.position.y = 0.15;
  home_pose.position.z = 0.5;
  home_pose.orientation.w = 0;
  home_pose.orientation.x = 1;
  home_pose.orientation.y =0;
  home_pose.orientation.z =0;

  // Call the plan_and_execute_pose function and use its return value
  if (this->plan_and_execute_pose(home_pose)) {
      response->success = true;
      response->message = "Moved to home (plan executed)";
  } else {
      response->success = false;
      response->message = "Failed to move to home (planning or execution failed)";
  }
}

//////////////////////////////////
///////// MOVEIT HANDLE //////////
//////////////////////////////////
bool FreddyNode::plan_and_execute_pose(const geometry_msgs::msg::Pose& target_pose) {

  RCLCPP_INFO(this->get_logger(), "Planning frame: %s", this->p_move_group_interface->getPlanningFrame().c_str());

  // We can also print the name of the end-effector link for this group.
  RCLCPP_INFO(this->get_logger(), "End effector link: %s", this->p_move_group_interface->getEndEffectorLink().c_str());

  // We can get a list of all the groups in the robot:
  RCLCPP_INFO(this->get_logger(), "Available Planning Groups:");
  std::copy(this->p_move_group_interface->getJointModelGroupNames().begin(), this->p_move_group_interface->getJointModelGroupNames().end(),
            std::ostream_iterator<std::string>(std::cout, ", "));

  this->p_move_group_interface->setPoseTarget(target_pose);


  moveit::planning_interface::MoveGroupInterface::Plan my_plan;

  bool success = (p_move_group_interface->plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);

  RCLCPP_INFO(this->get_logger(), "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");

  // Execute the plan
   if(success_plan) {
     moveit::core::MoveItErrorCode exec_result = this->p_move_group_interface->execute(plan);
     if (exec_result != moveit::core::MoveItErrorCode::SUCCESS) {
       RCLCPP_ERROR(this->get_logger(), "Execution failed!");
       return false;
     }
   } else {
     RCLCPP_ERROR(this->get_logger(), "Planning failed!");
    return false;
   }

  return true;

}


//////////////////////////////////
/////////// SETTERS //////////////
//////////////////////////////////
bool FreddyNode::setMoveGroupInterface(const std::shared_ptr<moveit::planning_interface::MoveGroupInterface>& interface){
  this->p_move_group_interface = interface;
  // Optionally check if interface is valid
  if (!this->p_move_group_interface) {
    RCLCPP_ERROR(this->get_logger(), "Bad initialisation of move group interface");
    return false;
  }
  return true;
}

When i run the code and i try to getCurrentPose() i get a message like:
Listening to joint states on topic 'joint_states'
Didn't receive robot state (joint angles) with recent timestamp within 1.00000 seconds. Requested time 175154603, but last recevied has time 0.000. Check Clock synchronisation ...
Failed to Fetch current robot state
Current Pose: 0,0,0,0,0,0,0

If i do ros2 node info on my node i clearly see it is not subscribing to Joint states, but if i echo joint states it is working fine. I have checked for namespaces and so on. No issues there.

I also get a no kinematics plugins defined warning, but the kinematics.yaml file works well in RViz.

Has anyone experiences a similar issue, i didn't seem to find it.

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions