two topics subscribed to one node c++ with sequance
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 ...
Could you please format the code? It is a bit hard to read
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
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.