Moveit using BehaviorTree

[ariccspstk]

I am trying to experiment behaviour tree with moveit instead of a state machine and right now i do not know why but it is always stuck in the ROS_INFO("hello i am moving"). Could this be a spin issue? or something with behavior tree? I am not sure

using namespace BT;

// Mutex for synchronizing access to the detected_objects vector
std::mutex detected_objects_mutex;
std::vector<geometry_msgs::PoseStamped> detected_objects;

// Callback function for the object detection subscriber
void ObjectDetectionCallback(const aricc_vision_msgs::ObjectArray::ConstPtr& msg)
{
    std::lock_guard<std::mutex> lock(detected_objects_mutex);

    int cnt = 0;
    for (auto& obj : msg->objects)
    {
        geometry_msgs::TransformStamped transformStamped;
        static tf2_ros::TransformBroadcaster tf_broadcaster;

        std::cout << "[" << ++cnt << "] Detect object: '" << obj.name << "'." << std::endl;

        geometry_msgs::PoseStamped pose;
        pose.header.frame_id = obj.name;
        pose.pose.position = obj.position;

        // Fix: Assign Quaternion components individually
        pose.pose.orientation.x = obj.orientation.x;
        pose.pose.orientation.y = obj.orientation.y;
        pose.pose.orientation.z = obj.orientation.z;

        transformStamped.header.stamp = ros::Time::now();
        transformStamped.header.frame_id = "camera_color_optical_frame"; // Parent frame
        transformStamped.child_frame_id = pose.header.frame_id; // Child frame

        transformStamped.transform.translation.x = -pose.pose.position.y; // Object position x
        transformStamped.transform.translation.y = -pose.pose.position.x; // Object position y
        transformStamped.transform.translation.z = pose.pose.position.z;

        tf2::Quaternion q;
        q.setRPY(1.57, -1.57, -pose.pose.orientation.z);
        transformStamped.transform.rotation.x = q.x(); // Rotation
        transformStamped.transform.rotation.y = q.y();
        transformStamped.transform.rotation.z = q.z();
        transformStamped.transform.rotation.w = q.w();

        detected_objects.push_back(pose);
        tf_broadcaster.sendTransform(transformStamped);
    }
}

// ObjectDetection class definition
class ObjectDetection : public BT::SyncActionNode
{
public:
    ObjectDetection(const std::string& name, const BT::NodeConfiguration& config)
        : BT::SyncActionNode(name, config), node_(""), listener_(buffer_)
    {
        // Subscriber for object detection
        sub_ = node_.subscribe("/re_dl_detection/YOLOv8RotateObjectDetection/dl_output", 1000, ObjectDetectionCallback);
    }

    // Define the input ports for the behavior tree node
  static BT::PortsList providedPorts()
{
    return {
        BT::InputPort<std::string>("object"),
        BT::OutputPort<geometry_msgs::Pose>("object_pose")
    };
}

    // Execute the behavior tree node logic
    virtual BT::NodeStatus tick() override
    {
        std::string object_name;
        ros::Duration(1).sleep();

        // Check if the required input "object" is missing
        if (!getInput<std::string>("object", object_name))
        {
            throw BT::RuntimeError("missing required input [pose]");
        }

        {
            std::lock_guard<std::mutex> lock(detected_objects_mutex);
            detected_objects.clear();
        }

        ros::spinOnce();

        auto predicate = [&object_name](const geometry_msgs::PoseStamped& detected_pose) {
            return detected_pose.header.frame_id == object_name;
        };

        {
            std::lock_guard<std::mutex> lock(detected_objects_mutex);
            int cnt = std::count_if(detected_objects.begin(), detected_objects.end(), predicate);

            if (cnt > 2)
            {
                try {
                    // Lookup the transform
                    buffer_.canTransform("base_link1", detected_objects[0].header.frame_id, ros::Time(0), ros::Duration(1.0));
    
                    // Now perform the transform lookup
                    transformStamped1 = buffer_.lookupTransform("base_link1", detected_objects[0].header.frame_id, ros::Time(0));
                } catch (tf2::TransformException &ex) {
                    ROS_ERROR("Failed to lookup transform: %s", ex.what());
                    return BT::NodeStatus::FAILURE;
                }
                // Extract and print the transform information
                move_target.position.x = transformStamped1.transform.translation.x;
                move_target.position.y = transformStamped1.transform.translation.y;
                move_target.position.z = transformStamped1.transform.translation.z;
                move_target.orientation.x = transformStamped1.transform.rotation.x;
                move_target.orientation.y = transformStamped1.transform.rotation.y;
                move_target.orientation.z = transformStamped1.transform.rotation.z;
                move_target.orientation.w = transformStamped1.transform.rotation.w;
                ROS_INFO_STREAM("Transform from base_link1 to " << detected_objects[0].header.frame_id << " received:");
                ROS_INFO_STREAM(move_target);
                setOutput<geometry_msgs::Pose>("object_pose" , move_target);
                return BT::NodeStatus::SUCCESS;
            }
        }

        return BT::NodeStatus::FAILURE;
    }

private:
    ros::NodeHandle node_;
    ros::Subscriber sub_;
    tf2_ros::Buffer buffer_;
    tf2_ros::TransformListener listener_;
    geometry_msgs::Pose move_target;
    geometry_msgs::TransformStamped transformStamped1;
};

class PickObject : public BT::SyncActionNode
{
public:
    PickObject(const std::string& name, const BT::NodeConfiguration& config)
        : BT::SyncActionNode(name, config) , move_group("gluon_arm") 
    {}

    // Define the input ports for the behavior tree node
 static BT::PortsList providedPorts()
{
    return {BT::InputPort<geometry_msgs::Pose>("PickObject")};
}
    // Execute the behavior tree node logic
    virtual BT::NodeStatus tick() override
    {
        if (!getInput<geometry_msgs::Pose>("PickObject", Pick_Object_))
        {
            throw BT::RuntimeError("missing required input [pose]");
        }
        ROS_INFO_STREAM(Pick_Object_);
        move_group.setPlanningTime(5.0);
        ROS_INFO("hello im moving");
         move_group.setPoseReferenceFrame("base_link1");
         move_group.setPoseTarget(Pick_Object_);
         Pick_Object_.position.z = 0.1;
         move_group.move();
         ROS_INFO("i have finish movinig");
         return BT::NodeStatus::SUCCESS;
    }

private:
  moveit::planning_interface::MoveGroupInterface move_group; 
  geometry_msgs::Pose Pick_Object_;

};
// Main function
int main(int argc, char** argv)
{
    ros::init(argc, argv, "test_bt");
    ros::NodeHandle nh;

    // Behavior Tree Factory
    BehaviorTreeFactory factory;
    factory.registerNodeType<ObjectDetection>("ObjectDetection");
    factory.registerNodeType<PickObject>("PickObject");


    // Create Behavior Tree from XML file
    auto tree = factory.createTreeFromFile("/home/ubuntu/catkin_ws_qt/src/test_node/cfg/bt_tree.xml");

    // Logger for console output
    StdCoutLogger logger_cout(tree);

    NodeStatus status = NodeStatus::RUNNING;

    // Run the Behavior Tree
    while (ros::ok() && status == NodeStatus::RUNNING)
    {   
        status = tree.rootNode()->executeTick();
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    return 0;
}

[facontidavide]

Sorry, I have no bandwidth to help people at this level.
If you think that there is a bug in the code, submit an issue here, otherwise use Discussion and community support.

[ariccspstk]

Ok thank you for the fast reply !

[facontidavide]

migrated to Discussions

[KentaKato]

@ariccspstk
Have you resolved it? You probably need to use an asynchronous spinner.

https://industrial-training-master.readthedocs.io/en/kinetic/_source/session4/Motion-Planning-CPP.html#:~:text=As%20described%20here,%2C%20as%20shown%3A