0
$\begingroup$

Rosanswers logo

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/ros/electric/stacks/pr2_controllers/ethercat_trigger_controllers/controller_plugins.xml
pr2_calibration_controllers /opt/ros/electric/stacks/pr2_controllers/pr2_calibration_controllers/controller_plugins.xml
pr2_mechanism_controllers /opt/ros/electric/stacks/pr2_controllers/pr2_mechanism_controllers/controller_plugins.xml
 robot_mechanism_controllers /opt/ros/electric/stacks/pr2_controllers/robot_mechanism_controllers/controller_plugins.xml
 pr2_controller_manager /opt/ros/electric/stacks/pr2_mechanism/pr2_controller_manager/test/controller_plugin.xml

Please, help. Thanks.


Originally posted by Peter Listov on ROS Answers with karma: 338 on 2012-08-28

Post score: 0


Original comments

Comment by Winston on 2014-08-20:
Can you tell us your solution?

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Problem solved.


Originally posted by Peter Listov with karma: 338 on 2012-08-28

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by Ros_user on 2015-08-28:
HOW have you solved it?

$\endgroup$

This site is temporarily in read-only mode and not accepting new answers.