two topics subscribed to one node c++ with sequance

asked 2022-09-06 15:54:50 -0500

Ab1010 gravatar image

updated 2022-09-07 06:37:04 -0500

Mike Scheutzow gravatar image

Hi,

I have an issue with subscribing to two topics in one node, I have two callbacks and I want to execute the void function in sequence. to make it clear,

the first subscriber has a void callback called target the second subscriber has a void callback called goal

so I would like to execute them in that sequence, the code below only executes the first void function and ignore the second void function. So, how do I execute the second void function as well?

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <boost/thread.hpp>
#include <geometry_msgs/Pose.h>

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <unistd.h>     //required for usleep()
#include <geometry_msgs/Pose.h>




geometry_msgs::Pose goal_pose;
geometry_msgs::Pose target_pose;



class multiThreadListener
{
public:
    multiThreadListener()
    {
        sub = n.subscribe("target_pose", 1, &multiThreadListener::target_callback, this);
        sub2 = n.subscribe("goal_pose", 1, &multiThreadListener::goal_callback, this);

    }

    void target_callback(const geometry_msgs::Pose &point);
    void goal_callback(const geometry_msgs::Pose &point1);



private:
    ros::NodeHandle n;
    ros::Subscriber sub;
  ros::Subscriber sub2;
};



void multiThreadListener::target_callback(const geometry_msgs::Pose &point)
{
  target_pose = point;

  ROS_INFO("     target (y)(%f)", target_pose.position.y);
  ROS_INFO("     target (X)(%f)", target_pose.position.x);
  sub.shutdown();
  //ros::Rate loop_rate(0.5);//block chatterCallback2()


  //loop_rate.sleep();
  //ros::spin(1);

  const int microToSeconds = 1000000;   
  const double delay1 = 1 * microToSeconds;     //35 seconds

  //ros::AsyncSpinner spinner(1);
  //spinner.start();

    static const std::string PLANNING_GROUP_ARM = "arm";
    static const std::string PLANNING_GROUP_GRIPPER = "gripper";

    // The :planning_interface:`MoveGroupInterface` class can be easily
    // setup using just the name of the planning group you would like to control and plan for.
    moveit::planning_interface::MoveGroupInterface move_group_interface_arm(PLANNING_GROUP_ARM);
    moveit::planning_interface::MoveGroupInterface move_group_interface_gripper(PLANNING_GROUP_GRIPPER);

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

    moveit::planning_interface::MoveGroupInterface::Plan my_plan_arm;

    // 1. Move to home position
    move_group_interface_arm.setJointValueTarget(move_group_interface_arm.getNamedTargetValues("home"));

    bool success = (move_group_interface_arm.plan(my_plan_arm) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

    ROS_INFO_NAMED("tutorial", "Move To Home Position %s", success ? "" : "FAILED");



    move_group_interface_arm.move();

    // 2. Place the TCP (Tool Center Point, the tip of the robot) above the blue box
    geometry_msgs::PoseStamped current_pose;
    current_pose = move_group_interface_arm.getCurrentPose("link_7");

    geometry_msgs::Pose target_pose1;

    //geometry_msgs::Pose target_pose;

    target_pose1.orientation = current_pose.pose.orientation;

    target_pose1.position.x = target_pose.position.x;
    target_pose1.position.y = target_pose.position.y;
    target_pose1.position.z = 0.2;

    //ROS_INFO("      before target (y)(%f)", target_pose.position.y);
    //ROS_INFO("     before target (X)(%f)", target_pose.position.x);

    move_group_interface_arm.setPoseTarget(target_pose1);

    success = (move_group_interface_arm.plan(my_plan_arm) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

    ROS_INFO_NAMED("tutorial", "Move To Goal %s", success ? "" : "FAILED");


    move_group_interface_arm.move();


    moveit::planning_interface::MoveGroupInterface::Plan my_plan_gripper;


    // 3. Move the TCP close to the object
    target_pose1.position.z = target_pose1.position.z - 0.0;
    move_group_interface_arm.setPoseTarget(target_pose1);

    success = (move_group_interface_arm.plan(my_plan_arm) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

    ROS_INFO_NAMED("tutorial", "Move The TCP To The Object %s", success ? "" : "FAILED");

    move_group_interface_arm.move();

    // 4. Close the  gripper
    move_group_interface_gripper.setJointValueTarget(move_group_interface_gripper.getNamedTargetValues("closed"));

    success = (move_group_interface_gripper.plan(my_plan_gripper) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

    ROS_INFO_NAMED("tutorial", "Close Gripper %s", success ? "" : "FAILED");

    move_group_interface_gripper.move();

    usleep(delay1); 


}

//////////////////////////////////////////////////////////////////////////

void multiThreadListener::goal_callback(const geometry_msgs ...
(more)
edit retag flag offensive close merge delete

Comments

Could you please format the code? It is a bit hard to read

Vic gravatar image Vic  ( 2022-09-07 03:12:14 -0500 )edit
1

What you can try :

  • rostopic echo your "goal_pose" topic. The callback won't be called if the topic is never published by another node.

  • If you still have issue despite correct publication, create a simple node that does only the subscription to "goal_pose" and callback to make sure everything is fine on the ROS side.

  • You have lots of code in your callbacks. Start by keeping it simple - for example, display a message. You might block your node for too long. I'd personally advise to keep callbacks simple - use them to set data, and maybe flags, asynchronously and do your main treatment in the main function (or better, a specilized sub-function called by main). Else, you might block other callbacks

Vic gravatar image Vic  ( 2022-09-07 03:19:27 -0500 )edit
1

This is a good comment, with a mention of http://wiki.ros.org/roscpp/Overview/C... it could be an answer. I suggest OP to read about message_filters package, because maybe it could help him achieve his goal.

ljaniec gravatar image ljaniec  ( 2022-09-07 07:34:18 -0500 )edit