Action_pub.publish(set_vel) not working

Software
1

Dear all,

I followed the tutorial ´visual object recognition´. I was able to recognize a picture ´ARROW_LEFT´. However, in the provided node ´action_controller.cpp’ the ROSbot should be able to drive after recognition, but it does not drive at all.

This is my code:

#include <ros/ros.h>
#include <std_msgs/Float32MultiArray.h>
#include <geometry_msgs/Twist.h>

#define ARROW_LEFT 6

int id = 0;
ros::Publisher action_pub;
geometry_msgs::Twist set_vel;

void objectCallback(const std_msgs::Float32MultiArrayPtr &object) {
if (object->data.size() > 0) {
id = object->data[0];

  switch (id) {
     case ARROW_LEFT:
        std::cout<<"photo recognized"<< std::endl;
        set_vel.linear.x = 0;
        set_vel.angular.z = 1;
        break;
     default: // other object
        set_vel.linear.x = 0;
        set_vel.angular.z = 0;
  }
  action_pub.publish(set_vel);

} else {
// No object detected
set_vel.linear.x = 0;
set_vel.angular.z = 0;
action_pub.publish(set_vel);
}
}

int main(int argc, char **argv) {

ros::init(argc, argv, “action_controller”);
ros::NodeHandle n("~");
ros::Rate loop_rate(50);
ros::Subscriber sub = n.subscribe("/objects", 1, objectCallback);
action_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
set_vel.linear.x = 0;
set_vel.linear.y = 0;
set_vel.linear.z = 0;
set_vel.angular.x = 0;
set_vel.angular.y = 0;
set_vel.angular.z = 0;
while (ros::ok()) {
ros::spinOnce();
loop_rate.sleep();
}
}

I know the photo is recognized because when I show the picture with ID 6 to the camera, it prints ’ photo recognized’. So the fault should be in set_vel.linear.x and set_vel.linear.y. I already tried several values for those msgs but the ROSbot did not respond to anything.

Can someone please tell me what I am doing wrong?

Kind regards,
Mark

2

Hi Mark,

I think You should verify that other parts of the system are up and running.

Please issue below command in new terminal window:

rostopic info /cmd_vel

This will print info regarding /cmd_vel topic. You should get response similar to:

Type: geometry_msgs/Twist

Publishers: 
 * /action_controller (http://10.5.10.122:44245/)

Subscribers: 
 * /serial_node (http://10.5.10.198:41952/)

You need exactly one publisher, which is the action controller node and one subscriber which is serial node to communicate with CORE2 board.

If you have it done, issue another command, while object is in front of camera:

rostopic echo /cmd_vel -n 1

This will print single message that is sent from publisher to subscriber. Output should be similar to:

linear: 
  x: 0.0
  y: 0.0
  z: 0.0
angular: 
  x: 0.0
  y: 0.0
  z: 1.0
---

Please check that values are the same as You set for selected object.

Last thing is code uploaded to CORE2 - this should be the same code as in tutorial 3: ‘Simple kinematics’.

Regards,
Łukasz