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?