static moveit::core::RobotModelPtr getModel()
{
static moveit::core::RobotModelPtr model;
if (!model)
{
boost::shared_ptr<urdf::ModelInterface> urdf(urdf::parseURDF(URDF_STR));
boost::shared_ptr<srdf::Model> srdf(new srdf::Model());
srdf->initString(*urdf, SRDF_STR);
model.reset(new moveit::core::RobotModel(urdf, srdf));
}
return model;
}
PanTiltJoy() {
std::string panTiltTopic, joySubTopic;
_tiltPos = _panPos = 0;
_deadManButtonActive = _zeroButtonActive = false;
boost::shared_ptr<urdf::Model> urdf(new urdf::Model);
if (!urdf->initParam("robot_description")) {
ROS_ERROR("[%s]: Need to have urdf model.", ros::this_node::getName().c_str());
ros::shutdown();
}
tiltLim = urdf->getJoint("head_tilt_joint");
panLim = urdf->getJoint("head_pan_joint");
if (!tiltLim) {
ROS_ERROR("[%s]: Could not find head_tilt_joint.", ros::this_node::getName().c_str());
ros::shutdown();
}
if (!panLim) {
ROS_ERROR("[%s]: Could not find head_pan_joint.", ros::this_node::getName().c_str());
ros::shutdown();
}
// Validation check.
if (!_nodeHandle.getParam("pan_tilt_topic", panTiltTopic)
|| !_nodeHandle.getParam("joy_sub_topic", joySubTopic)
|| !_nodeHandle.getParam("joy_pan_axis", _panAxisIndex)
|| !_nodeHandle.getParam("joy_tilt_axis", _tiltAxisIndex)
|| !_nodeHandle.getParam("increament_tilt", _incTilt)
|| !_nodeHandle.getParam("increament_pan", _incPan)
|| !_nodeHandle.getParam("joy_deadman_button", _deadManButtonIndex)
|| !_nodeHandle.getParam("zero_button", _zeroButtonIndex)) {
ROS_ERROR(
"[%s]: Requird: pan_tilt_topic, joy_sub_topic, joy_pan_axis, joy_tilt_axis, increament_tilt, increament_pan , zero_button, joy_deadman_button parameters",
ros::this_node::getName().c_str());
ros::shutdown();
}
else {
_panTiltCommand = _nodeHandle.advertise<trajectory_msgs::JointTrajectory>(panTiltTopic, 10);
_joySub = _nodeHandle.subscribe<sensor_msgs::Joy>(joySubTopic, 10, &PanTiltJoy::joyCallback, this);
}
}
