Ask Your Question
0

cannot subscribe plugin

asked 2012-09-03 07:03:32 -0500

Peter Listov gravatar image

Hallo! I want my plugin (joint controller) to be subscribed to the publisher node, which publishes messages of that type:

            int64 Acceleration
            float64 Angle

So the problem is i cannot subscribe to them. Here is my code for plugin^ .h - file:

   #include <pr2_controller_interface/controller.h>
   #include <pr2_mechanism_model/joint.h>
   #include "/home/robot/peter_workspace/sandbox/drive_base/msg_gen/cpp/include/drive_base/Drive.h"
   #include "ros/ros.h"

   namespace my_controller_ns{

   class MyControllerClass: public pr2_controller_interface::Controller
   {
   private:

     ros::NodeHandle n;
     ros::Subscriber robocv_controller; 
     ros::Publisher drive_cmd_publisher;

     pr2_mechanism_model::JointState* left_wheel_state_;
     pr2_mechanism_model::JointState* right_wheel_state_;
     double init_pos1_;
     double init_pos2_;

   public:

     //int64_t acceleration_ctrl;
     //double angle_ctrl;

     virtual bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &nh);
     virtual void starting();
     virtual void update();
     virtual void stopping();
     virtual void chatterCallback(const drive_base::Drive::ConstPtr& msg);      
   };
   }

And the CPP - file:

  #include "robocv_controller/robocv_controller_file.h"
   #include <pluginlib/class_list_macros.h>
   #include "/home/robot/peter_workspace/sandbox/drive_base/msg_gen/cpp/include/drive_base/Drive.h"
   #include "ros/ros.h"


namespace my_controller_ns{

   void chatterCallback(const drive_base::Drive::ConstPtr& msg)
   {
    ROS_INFO("i g0t %lld %f", msg->Acceleration, msg->Angle);
    //acceleration_ctrl = msg->Acceleration;
    //angle_ctrl = msg->Angle;
   }

   /// Controller initialization in non-realtime
   bool MyControllerClass::init(pr2_mechanism_model::RobotState *robot,
                               ros::NodeHandle &nh)
   {
     n = nh;

     robocv_controller = n.subscribe("/command", 100, &MyControllerClass::chatterCallback, this);

     std::string left_wheel, right_wheel;

     if (!n.getParam("left_wheel", left_wheel))
     {
       ROS_ERROR("No joint given in namespace: '%s')",
                 n.getNamespace().c_str());
       return false;
     }

     left_wheel_state_ = robot->getJointState(left_wheel);
     if (!left_wheel_state_)
     {
       ROS_ERROR("MyController could not find joint named '%s'",
                 left_wheel.c_str());
       return false;
     }

/////////////another wheel/////////////////////

     if (!n.getParam("right_wheel", right_wheel))
     {
       ROS_ERROR("No joint given in namespace: '%s')",
                 n.getNamespace().c_str());
       return false;
     }

     right_wheel_state_ = robot->getJointState(right_wheel);
     if (!right_wheel_state_)
     {
       ROS_ERROR("MyController could not find joint named '%s'",
                 right_wheel.c_str());
       return false;
     }
     return true;
   }


   /// Controller startup in realtime
   void MyControllerClass::starting()
   {
     init_pos1_ = left_wheel_state_->position_;
     init_pos2_ = right_wheel_state_->position_;
   }


   /// Controller update loop in realtime
   void MyControllerClass::update()
   {
    // double desired_pos = init_pos1_ + 15 * sin(ros::Time::now().toSec());
     double desired_pos = 0.45;
     double current_pos = left_wheel_state_->position_;
     double current_pos_right = right_wheel_state_->position_;
     left_wheel_state_->commanded_effort_ = -150 * (current_pos - desired_pos);
     right_wheel_state_->commanded_effort_ = -150 * (current_pos_right - desired_pos);
   }


   /// Controller stopping in realtime
   void MyControllerClass::stopping()
   {}

} //namespace

PLUGINLIB_DECLARE_CLASS(controller,MyControllerPlugin, 
                         my_controller_ns::MyControllerClass, 
                         pr2_controller_interface::Controller)

Thank you for help!

edit retag flag offensive close merge delete

Comments

Thanks, dornhege! It was totally my fault- forgot to mention class identifier before chatterCallback() function: void MyControllerClass::chatterCallback(const drive_base::Drive::ConstPtr& msg). Now everything works fine.

Peter Listov gravatar imagePeter Listov ( 2012-09-03 23:54:41 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2012-09-03 07:57:34 -0500

dornhege gravatar image

Something is wrong somewhere. You need to give some more information why. These are things I would suspect: Is the plugin loaded? Verify via printout or similar. Is someone spinning to receive messages?

PS: You should NEVER give hardcoded paths in code. drive_base/Drive.h should be sufficient as an include (put drive_base as a depends in the manifest).

edit flag offensive delete link more

Comments

when i do make, the drive_base depends in my manifest doesn't work..."non-existent package drive_base"

Marlon Rocha gravatar imageMarlon Rocha ( 2013-04-02 05:23:49 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2012-09-03 07:03:32 -0500

Seen: 159 times

Last updated: Sep 03 '12