3 #include <boost/thread/condition.hpp> 18 std::string joint_name;
19 if (!n.
getParam(
"joint_name", joint_name))
27 ROS_ERROR(
"MyController could not find joint named '%s'",
33 ROS_ERROR(
"MyController could not find limits of joint '%s'",
43 std::string root_name, tip_name;
44 if (!n.
getParam(
"root_name", root_name))
49 if (!n.
getParam(
"tip_name", tip_name))
60 std::string topic_name;
61 if (!n.
getParam(
"topic_name", topic_name))
67 pub_->msg_.name.resize(1);
68 pub_->msg_.name[0] =
"";
69 pub_->msg_.effort.resize(1);
70 pub_->msg_.effort[0] = 0.0;
75 std::string service_name;
76 if (!n.
getParam(
"service_name", service_name))
100 pub_->unlockAndPublish();
123 pr2_mechanism_msgs::LoadController::Response& resp)
125 pub_->msg_.name[0] = req.name;
PLUGINLIB_EXPORT_CLASS(my_controller_ns::MyControllerClass, pr2_controller_interface::Controller)
Register controller to pluginlib.
pr2_mechanism_model::Chain chain_
ros::Time time_of_last_cycle_
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
boost::shared_ptr< const urdf::Joint > joint_
void starting()
Controller startup in realtime.
const std::string & getNamespace() const
JointState * getJointState(const std::string &name)
boost::scoped_ptr< realtime_tools::RealtimePublisher< sensor_msgs::JointState > > pub_
bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
Controller initialization in non-realtime.
void stopping()
Controller stopping in realtime.
void update()
Controller update loop in realtime.
bool getParam(const std::string &key, std::string &s) const
pr2_mechanism_model::JointState * joint_state_
bool init(RobotState *robot_state, const std::string &root, const std::string &tip)
pr2_mechanism_model::RobotState * robot_
bool serviceCallback(pr2_mechanism_msgs::LoadController::Request &req, pr2_mechanism_msgs::LoadController::Response &resp)
Service call.
void toKDL(KDL::Chain &chain)