Robotics StackExchange | Archived questions

controller and ros manager

hello everyone : I am using the ros controller , and I have wrote a controller and a controller manager , but when I want to load the controller to ros manager , I get some problem:

[ERROR] [1431447499.444484032]: Could not load class drive_controller/DriveController: Failed to load library /home/exbot/Documents/ROS/control_error/devel/lib//libdrive_controller.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = /home/exbot/Documents/ROS/control_error/devel/lib//libdrive_controller.so: undefined symbol: _ZTVN16drive_controller15DriveControllerE)
[ERROR] [1431447499.444575741]: Could not load controller 'exp_drive_controller' because controller type 'drive_controller/DriveController' does not exist.
[ERROR] [1431447499.444601932]: Use 'rosservice call controller_manager/list_controller_types' to get the available types

[ERROR] [WallTime: 1431447500.445374] Failed to load exp_drive_controller

and this is drive_controller's CMakeLists.txt:

  cmake_minimum_required(VERSION 2.8.3)
project(drive_controller)

find_package(catkin REQUIRED COMPONENTS
  controller_interface
  hardware_interface
  realtime_tools
  roscpp
  pluginlib
  rospy
  tf
  urdf
)

 find_package(Boost REQUIRED COMPONENTS system)


catkin_package(
  INCLUDE_DIRS include
  LIBRARIES drive_controller
  CATKIN_DEPENDS controller_interface hardware_interface realtime_tools roscpp rospy tf urdf
  DEPENDS system_lib
)

include_directories(include)
include_directories(
  ${catkin_INCLUDE_DIRS}
)

 add_library(${PROJECT_NAME}
   src/drive_controller.cpp
 )

 target_link_libraries(${PROJECT_NAME}
   ${catkin_LIBRARIES}
 )

 install(TARGETS ${PROJECT_NAME}
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )


 install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
   FILES_MATCHING PATTERN "*.h"
   PATTERN ".svn" EXCLUDE
 )

 install(FILES
   driver_controller.xml
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )

and this is the xml for the plugin:

    <library path="lib/libdrive_controller">
  <class name="drive_controller/DriveController" type="drive_controller::DriveController" base_class_type="controller_interface::ControllerBase">
    <description>

    </description>
  </class>

</library>

and this my manager's .launch file:

    <launch>
    <rosparam file="$(find exp_hardware)/config/exp_controller.yaml" command="load"/>
    <node name="spawner" pkg="controller_manager" type="spawner" args="exp_drive_controller " respawn="false"/>
    <node name="exp_hard_soft" pkg="exp_hardware" type="exp_hardware" output="screen"/>
</launch>

and this is my manager's yaml file:

    exp_drive_controller:
 type: "drive_controller/DriveController"
 left_wheel_name: 'left_wheel'
 right_wheel_name: 'right_wheel'

the code for the drive controller is :

#include <geometry_msgs/Twist.h>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <pluginlib/class_list_macros.h>
#include <tf/tfMessage.h>
#include <realtime_tools/realtime_buffer.h>
#include <realtime_tools/realtime_publisher.h>
#include <string>

using namespace std ;
using namespace controller_interface ;

typedef struct
{
    double linear_x_vel ;
    double angular_vel ;
    ros::Time current_time ;
}Commands ;

namespace drive_controller {
class DriveController:
       public Controller<hardware_interface::VelocityJointInterface>
{
public:
    DriveController() ;
    ~DriveController();
    bool init(hardware_interface::VelocityJointInterface *hw,
              ros::NodeHandle &root_nh,
              ros::NodeHandle &controller_nh);
    void update(const ros::Time &time, const ros::Duration &period);
    void starting(const ros::Time &time) ;
    void stopping(const ros::Time &time) ;
protected:
private:
    double wheel_radius_ ;
    double wheel_separation_ ;
    string left_wheel_name ;
    string right_wheel_name ;
    string name_ ;

    double cmd_vel_timeout ;

    hardware_interface::JointHandle left_wheel_joint ;
    hardware_interface::JointHandle right_wheel_joint ;
    realtime_tools::RealtimeBuffer<Commands> realtime_commands ;
    Commands current_command ;
    Commands last_command ;

    ros::Subscriber cmd_vel_sub ;
   // ros::Time last_state_time_ ;
    enum {STARTING , STOPPING , RUNNING} STATE ;
private:
     void cmdvelCallBack(const geometry_msgs::TwistConstPtr ptr) ;
     bool getWheelNames(ros::NodeHandle& controller_nh,
             const std::string& wheel_param,
             std::string& wheel_name);
     void brake() ;
};
}


#include <drive_controller/drive_controller.h>
#include <boost/make_shared.hpp>
#include <pluginlib/class_list_macros.h>

namespace drive_controller {
DriveController::DriveController()
{
    wheel_radius_ = 0.9 ;
    wheel_separation_ = 1.2;
    current_command.angular_vel = 0 ;
    current_command.linear_x_vel  = 0 ;
    current_command.current_time.init() ;
}

bool DriveController::init(hardware_interface::VelocityJointInterface *hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
{    
    const std::string complete_ns = controller_nh.getNamespace();
        std::size_t id = complete_ns.find_last_of("/");
        name_ = complete_ns.substr(id + 1);

        if (!getWheelNames(controller_nh, "left_wheel_name",left_wheel_name) ||
                    !getWheelNames(controller_nh, "right_wheel_name", right_wheel_name))
            {
                return false;
            }


    left_wheel_joint  = hw->getHandle(left_wheel_name) ;
    right_wheel_joint = hw->getHandle(right_wheel_name) ;
    cmd_vel_sub = controller_nh.subscribe("/control_show/cmd_vel" , 10 , &DriveController::cmdvelCallBack ,this ) ;
    cout<<" the controller is ok!!!!"<<endl ;
}

void DriveController::starting(const ros::Time &time)
{
    brake();
 //   last_state_time_ = time ;
}

void DriveController::update(const ros::Time &time, const ros::Duration &period)
{
    current_command = *(realtime_commands.readFromRT()) ;
    double dt = (time - current_command.current_time).toSec() ;

    if(dt > cmd_vel_timeout)
    {
        current_command.linear_x_vel = 0.0 ;
        current_command.angular_vel =  0.0 ;
    }

    double left_wheel_vel = (current_command.linear_x_vel - current_command.angular_vel*wheel_separation_/2.0)/wheel_radius_ ;
    double right_wheel_vel = (current_command.linear_x_vel + current_command.angular_vel*wheel_separation_/2.0)/wheel_radius_ ;

    left_wheel_joint.setCommand(right_wheel_vel);
    right_wheel_joint.setCommand(right_wheel_vel);
    cout<<" set the vel"<<endl ;
}

void DriveController::stopping(const ros::Time &time)
{
    brake();
}

void DriveController::brake()
{
    double left_wheel_vel = 0 ;
    double right_wheel_vel = 0 ;
    left_wheel_joint.setCommand(left_wheel_vel);
    right_wheel_joint.setCommand(right_wheel_vel);
}

void DriveController::cmdvelCallBack(const geometry_msgs::TwistConstPtr ptr)
{
    last_command.linear_x_vel = ptr->linear.x ;
    last_command.angular_vel = ptr->angular.z ;
    last_command.current_time = ros::Time::now() ;
    realtime_commands.writeFromNonRT(last_command);

    ROS_INFO_STREAM("oh"<<"linear = :"<<last_command.linear_x_vel
                    <<" , "<<"angular = :"<<last_command.angular_vel
                    <<" , "<<"time = :"<<last_command.current_time) ;
}

bool DriveController::getWheelNames(ros::NodeHandle &controller_nh, const string &wheel_param, string &wheel_name)
{
    XmlRpc::XmlRpcValue wheel_list;
    ROS_INFO("get wheel name");
        if (!controller_nh.getParam(wheel_param, wheel_list))
        {
            ROS_ERROR_STREAM_NAMED(name_,
                    "Couldn't retrieve wheel param '" << wheel_param << "'.");
            return false;
        }

        if (wheel_list.getType() != XmlRpc::XmlRpcValue::TypeString)
        {
            ROS_ERROR_STREAM_NAMED(name_,
                    "Wheel param '" << wheel_param << "' #" <<
                    " isn't a string.");
            return false;
        }

        wheel_name = static_cast<std::string>(wheel_list);
        return true;
}
}
PLUGINLIB_EXPORT_CLASS(drive_controller::DriveController,controller_interface::ControllerBase)

Asked by Anthony Brooks on 2015-05-12 11:30:33 UTC

Comments

Answers