test_controllers.cpp
Go to the documentation of this file.
00001 
00026 #include "sr_mechanism_controllers/test/test_controllers.hpp"
00027 
00028 TestControllers::TestControllers()
00029 {
00030 }
00031 
00032 TestControllers::~TestControllers()
00033 {
00034 }
00035 
00036 void TestControllers::init()
00037 {
00038   hw = boost::shared_ptr<pr2_hardware_interface::HardwareInterface>( new pr2_hardware_interface::HardwareInterface() );
00039 
00040   //add a fake FFJ3 actuator
00041   actuator = boost::shared_ptr<sr_actuator::SrActuator>( new sr_actuator::SrActuator("FFJ3") );
00042   actuator->state_.is_enabled_ = true;
00043   hw->addActuator( actuator.get() );
00044 
00045   robot = boost::shared_ptr<pr2_mechanism_model::Robot>( new pr2_mechanism_model::Robot( hw.get()) );
00046 
00047   model = boost::shared_ptr<TiXmlDocument>( new TiXmlDocument() );
00048 
00049   ros::NodeHandle rosnode;
00050   std::string urdf_param_name;
00051   std::string urdf_string;
00052   // search and wait for robot_description on param server
00053   while(urdf_string.empty())
00054   {
00055     ROS_DEBUG("Waiting for urdf: %s on the param server.", "sh_description");
00056     if (rosnode.searchParam("sh_description",urdf_param_name))
00057     {
00058       rosnode.getParam(urdf_param_name,urdf_string);
00059       ROS_DEBUG("found upstream\n%s\n------\n%s\n------\n%s","sh_description",urdf_param_name.c_str(),urdf_string.c_str());
00060     }
00061     else
00062     {
00063       rosnode.getParam("sh_description",urdf_string);
00064       ROS_DEBUG("found in node namespace\n%s\n------\n%s\n------\n%s","sh_description",urdf_param_name.c_str(),urdf_string.c_str());
00065     }
00066     usleep(100000);
00067   }
00068   ROS_DEBUG("Parsing xml...");
00069 
00070   // initialize TiXmlDocument doc with a string
00071   if (!model->Parse(urdf_string.c_str()) && model->Error())
00072   {
00073     ROS_ERROR("Failed to parse urdf: %s\n",
00074             urdf_string.c_str());
00075   }
00076   else
00077   {
00078     robot->initXml( model->RootElement() );
00079 
00080     robot_state = boost::shared_ptr<pr2_mechanism_model::RobotState>( new pr2_mechanism_model::RobotState(robot.get()) );
00081 
00082     joint_state = robot_state->getJointState("FFJ3");
00083     joint_state->calibrated_ = true;
00084 
00085     init_controller();
00086     controller->starting();
00087 
00088     //initialize the controller:
00089     joint_state->position_ = 0.0;
00090     controller->update();
00091   }
00092 }
00093 
00094 /* For the emacs weenies in the crowd.
00095    Local Variables:
00096    c-basic-offset: 2
00097    End:
00098 */


sr_mechanism_controllers
Author(s): Ugo Cupcic / ugo@shadowrobot.com , contact@shadowrobot.com
autogenerated on Fri Jan 3 2014 12:06:50