Ask Your Question
0

controller fails to initialize

asked 2012-08-28 06:33:32 -0500

Peter Listov gravatar image

Hallo!

I've created a simple joint-controller as described in this tutorial: link text

Here is the code robocv_controller_file.h

   #include <pr2_controller_interface/controller.h>
   #include <pr2_mechanism_model/joint.h>

   namespace my_controller_ns{

   class MyControllerClass: public pr2_controller_interface::Controller
    {
      private:
       pr2_mechanism_model::JointState* left_wheel_state_;
         pr2_mechanism_model::JointState* right_wheel_state_;
         double init_pos1_;
         double init_pos2_;

     public:
     virtual bool init(pr2_mechanism_model::RobotState *robot,
                  ros::NodeHandle &n);
      virtual void starting();
       virtual void update();
      virtual void stopping();
    };
    }

robocv_controller_file.cpp

include "robocv_controller/robocv_controller_file.h"

#include <pluginlib class_list_macros.h="">

namespace my_controller_ns {

/// Controller initialization in non-realtime bool MyControllerClass::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n) { 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;
 }

//////////////////////////////////////////////////////////////////// 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 current_pos = left_wheel_state_->position_; left_wheel_state_->commanded_effort_ = -10 * (current_pos - desired_pos); right_wheel_state_->commanded_effort_ = -10 * (current_pos - desired_pos); }

/// Controller stopping in realtime void MyControllerClass::stopping() {} } // namespace PLUGINLIB_DECLARE_CLASS(controller,MyControllerPlugin, my_controller_ns::MyControllerClass, pr2_controller_interface::Controller)

It works fine. Then i wrote another one:

   #include <pr2_controller_interface/controller.h>
   #include <pr2_mechanism_model/joint.h>

    namespace my_controller_ns{

    class MyControllerClass: public pr2_controller_interface::Controller
    {
     private:
      pr2_mechanism_model::JointState* joint_state_;
      double init_pos_;

      public:
      virtual bool init(pr2_mechanism_model::RobotState *robot,
                 ros::NodeHandle &n);
       virtual void starting();
       virtual void update();
       virtual void stopping();
        };
        }

And appropriate C++ file:

#include "wheel_controller/wheel_controller_file.h"

#include <pluginlib class_list_macros.h="">

namespace my_controller_ns {

/// Controller initialization in non-realtime bool MyControllerClass::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n) { std::string joint_name; if (!n.getParam("/joint_name", joint_name)) { ROS_ERROR("No joint given in namespace: '%s')", n.getNamespace().c_str()); return false; }

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

}

/// Controller startup in realtime void MyControllerClass::starting() { init_pos_ = joint_state_->position_; }

/// Controller update loop in realtime void MyControllerClass::update() { double desired_pos = init_pos_ + 15 * sin(ros::Time::now().toSec()); double current_pos = joint_state_->position_; joint_state_->commanded_effort_ = -10 * (current_pos - desired_pos); }

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

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

When i'm trying to load the second controller rxconsole shows me this error:

Node: /gazebo
Time: 1365.017000000
Severity: Error
Location: /home/robot/peter_workspace/sandbox/controller        /src/robocv_controller_file.cpp:MyControllerClass::init:16
 Published Topics: /rosout, /tf, /gazebo/paused, /clock, /gazebo/link_states, /    gazebo/model_states, /gazebo/parameter_descriptions, /gazebo/parameter_updates,   /joint_states, /mechanism_statistics

 No joint given in namespace: '/wheel_controller')

For some reason it refers to robocv_controller source file. rospack plugins shows that both plugins are registered:

robot@Robot2:~/peter_workspace/sandbox/wheel_controller$ rospack plugins --attrib=plugin pr2_controller_interface
controller /home/robot/peter_workspace/sandbox/controller/controller_plugins.xml
velocity_controller /home/robot/peter_workspace/sandbox/velocity_controller/controller_plugins.xml
wheel_controller /home/robot/peter_workspace/sandbox/wheel_controller/controller_plugins.xml
ethercat_trigger_controllers /opt ...
(more)
edit retag flag offensive close merge delete

Comments

1

Can you tell us your solution?

Winston gravatar imageWinston ( 2014-08-20 15:02:50 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2012-08-28 23:35:59 -0500

Peter Listov gravatar image

Problem solved.

edit flag offensive delete link more

Comments

HOW have you solved it?

Ros_user gravatar imageRos_user ( 2015-08-28 05:48:50 -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

Stats

Asked: 2012-08-28 06:33:32 -0500

Seen: 612 times

Last updated: Aug 28 '12