0
$\begingroup$

Rosanswers logo

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!


Originally posted by Peter Listov on ROS Answers with karma: 338 on 2012-09-03

Post score: 0


Original comments

Comment by Peter Listov on 2012-09-03:
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.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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).


Originally posted by dornhege with karma: 31395 on 2012-09-03

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by Marlon Rocha on 2013-04-02:
when i do make, the drive_base depends in my manifest doesn't work..."non-existent package drive_base"

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.