SRNode(const ros::NodeHandle& nh): nh_(nh), it_d_(nh), it_i_(nh), it_c_(nh)
{
signal(SIGSEGV, &sigsegv_handler);
info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>("camera_info", 1);
image_pub_d_ = it_d_.advertise("distance/image_raw", 1);
image_pub_i_ = it_i_.advertise("intensity/image_raw", 1);
image_pub_c_ = it_c_.advertise("confidence/image_raw", 1);
image_pub_d16_ = it_c_.advertise("distance/image_raw16", 1);
cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud>("pointcloud_raw", 1);
cloud_pub2_ = nh_.advertise<sensor_msgs::PointCloud2>("pointcloud2_raw", 1);
// set reconfigurable parameter defaults (all to automatic)
// None right now
try
{
dev_ = new sr::SR(use_filter_);
}
catch (sr::Exception& e)
{
ROS_ERROR_STREAM("Exception thrown while constructing camera driver: "
<< e.what ());
nh_.shutdown();
return;
}
getInitParams();
}
void readLineInput(char input[30])
{
std::cin.getline(input, 30);
std::string inputStr(input);
if (boost::iequals(inputStr, "q"))
{
rosNode.shutdown();
std::exit(EXIT_SUCCESS);
}
}
void LoopbackControllerManager::fini()
{
ROS_DEBUG("calling LoopbackControllerManager::fini");
//pr2_hardware_interface::ActuatorMap::const_iterator it;
//for (it = hw_.actuators_.begin(); it != hw_.actuators_.end(); ++it)
// delete it->second; // why is this causing double free corrpution?
cm_->~ControllerManager();
rosnode_->shutdown();
ros_spinner_thread_->join();
}
UsbCamNode() :
node_("~")
{
// specify settings for images and camera for Realsense camera
image_width_ = 640;
image_height_ = 480;
framerate_ = 30;
// advertise the main image topic
image_transport::ImageTransport it(node_);
image_pub_ = it.advertiseCamera("image_raw", 1);
// grab the parameters
node_.param("video_device", video_device_name_, std::string("/dev/video0"));
// possible values: mmap, read, userptr
node_.param("io_method", io_method_name_, std::string("mmap"));
// load the camera info
node_.param("camera_frame_id", img_.header.frame_id, std::string("realsense_camera"));
node_.param("camera_name", camera_name_, std::string("realsense_camera"));
node_.param("camera_info_url", camera_info_url_, std::string(""));
cinfo_.reset(new camera_info_manager::CameraInfoManager(node_, camera_name_, camera_info_url_));
// check for default camera info
if (camera_info_url_.size() == 0)
{
cinfo_->setCameraName(video_device_name_);
sensor_msgs::CameraInfo camera_info;
camera_info.header.frame_id = img_.header.frame_id;
camera_info.width = image_width_;
camera_info.height = image_height_;
cinfo_->setCameraInfo(camera_info);
}
ROS_INFO("Starting '%s' (%s) at %dx%d, %i FPS", camera_name_.c_str(), video_device_name_.c_str(),
image_width_, image_height_, framerate_);
// set the IO method
UsbCam::io_method io_method = UsbCam::io_method_from_string(io_method_name_);
if(io_method == UsbCam::IO_METHOD_UNKNOWN)
{
ROS_FATAL("Unknown IO method '%s'", io_method_name_.c_str());
node_.shutdown();
return;
}
// start the camera
cam_.start(video_device_name_.c_str(), io_method, image_width_, image_height_, framerate_);
}
bool TaskInverseKinematics::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
{
nh_ = n;
// get URDF and name of root and tip from the parameter server
std::string robot_description, root_name, tip_name;
if (!ros::param::search(n.getNamespace(),"robot_description", robot_description))
{
ROS_ERROR_STREAM("TaskInverseKinematics: No robot description (URDF) found on parameter server ("<<n.getNamespace()<<"/robot_description)");
return false;
}
if (!nh_.getParam("root_name", root_name))
{
ROS_ERROR_STREAM("TaskInverseKinematics: No root name found on parameter server ("<<n.getNamespace()<<"/root_name)");
return false;
}
if (!nh_.getParam("tip_name", tip_name))
{
ROS_ERROR_STREAM("TaskInverseKinematics: No tip name found on parameter server ("<<n.getNamespace()<<"/tip_name)");
return false;
}
ROS_INFO("robot_description: %s",robot_description.c_str());
ROS_INFO("root_name: %s",root_name.c_str());
ROS_INFO("tip_name: %s",tip_name.c_str());
// Get the gravity vector (direction and magnitude)
KDL::Vector gravity_ = KDL::Vector::Zero();
gravity_(2) = -9.81;
// Construct an URDF model from the xml string
std::string xml_string;
if (n.hasParam(robot_description))
n.getParam(robot_description.c_str(), xml_string);
else
{
ROS_ERROR("Parameter %s not set, shutting down node...", robot_description.c_str());
n.shutdown();
return false;
}
if (xml_string.size() == 0)
{
ROS_ERROR("Unable to load robot model from parameter %s",robot_description.c_str());
n.shutdown();
return false;
}
ROS_DEBUG("%s content\n%s", robot_description.c_str(), xml_string.c_str());
// Get urdf model out of robot_description
urdf::Model model;
if (!model.initString(xml_string))
{
ROS_ERROR("Failed to parse urdf file");
n.shutdown();
return false;
}
ROS_DEBUG("Successfully parsed urdf file");
KDL::Tree kdl_tree_;
if (!kdl_parser::treeFromUrdfModel(model, kdl_tree_))
{
ROS_ERROR("Failed to construct kdl tree");
n.shutdown();
return false;
}
// Populate the KDL chain
if(!kdl_tree_.getChain(root_name, tip_name, kdl_chain_))
{
ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
ROS_ERROR_STREAM(" "<<root_name<<" --> "<<tip_name);
ROS_ERROR_STREAM(" Tree has "<<kdl_tree_.getNrOfJoints()<<" joints");
ROS_ERROR_STREAM(" Tree has "<<kdl_tree_.getNrOfSegments()<<" segments");
ROS_ERROR_STREAM(" The segments are:");
KDL::SegmentMap segment_map = kdl_tree_.getSegments();
KDL::SegmentMap::iterator it;
for( it=segment_map.begin(); it != segment_map.end(); it++ )
ROS_ERROR_STREAM( " "<<(*it).first);
return false;
}
ROS_DEBUG("Number of segments: %d", kdl_chain_.getNrOfSegments());
ROS_DEBUG("Number of joints in chain: %d", kdl_chain_.getNrOfJoints());
// Parsing joint limits from urdf model
boost::shared_ptr<const urdf::Link> link_ = model.getLink(tip_name);
boost::shared_ptr<const urdf::Joint> joint_;
joint_limits_.min.resize(kdl_chain_.getNrOfJoints());
joint_limits_.max.resize(kdl_chain_.getNrOfJoints());
joint_limits_.center.resize(kdl_chain_.getNrOfJoints());
int index;
std::cout<< "kdl_chain_.getNrOfJoints(): " << kdl_chain_.getNrOfJoints() << std::endl;
for (int i = 0; i < kdl_chain_.getNrOfJoints() && link_; i++)
{
joint_ = model.getJoint(link_->parent_joint->name);
index = kdl_chain_.getNrOfJoints() - i - 1;
joint_limits_.min(index) = joint_->limits->lower;
joint_limits_.max(index) = joint_->limits->upper;
joint_limits_.center(index) = (joint_limits_.min(index) + joint_limits_.max(index))/2;
link_ = model.getLink(link_->getParent()->name);
}
ROS_INFO("Successfully parsed joint limits");
// Get joint handles for all of the joints in the chain
for(std::vector<KDL::Segment>::const_iterator it = kdl_chain_.segments.begin()+1; it != kdl_chain_.segments.end(); ++it)
{
joint_handles_.push_back(robot->getHandle(it->getJoint().getName()));
ROS_DEBUG("%s", it->getJoint().getName().c_str() );
}
ROS_DEBUG(" Number of joints in handle = %lu", joint_handles_.size() );
PIDs_.resize(kdl_chain_.getNrOfJoints());
// Parsing PID gains from YAML
std::string pid_ = ("pid_");
for (int i = 0; i < joint_handles_.size(); ++i)
{
if (!PIDs_[i].init(ros::NodeHandle(n, pid_ + joint_handles_[i].getName())))
{
ROS_ERROR("Error initializing the PID for joint %d",i);
return false;
}
}
jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
id_solver_.reset(new KDL::ChainDynParam(kdl_chain_,gravity_));
fk_pos_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
ik_vel_solver_.reset(new KDL::ChainIkSolverVel_pinv(kdl_chain_));
ik_pos_solver_.reset(new KDL::ChainIkSolverPos_NR_JL(kdl_chain_,joint_limits_.min,joint_limits_.max,*fk_pos_solver_,*ik_vel_solver_));
joint_msr_states_.resize(kdl_chain_.getNrOfJoints());
joint_des_states_.resize(kdl_chain_.getNrOfJoints());
tau_cmd_.resize(kdl_chain_.getNrOfJoints());
J_.resize(kdl_chain_.getNrOfJoints());
sub_command_ = nh_.subscribe("command_configuration", 1, &TaskInverseKinematics::command_configuration, this);
return true;
}
void shutdown(){
timer_dsa.stop();
timer_publish.stop();
timer_diag.stop();
nh_.shutdown();
}
UsbCamNode() :
node_("~")
{
// advertise the main image topic
image_transport::ImageTransport it(node_);
image_pub_ = it.advertiseCamera("image_raw", 1);
// grab the parameters
node_.param("video_device", video_device_name_, std::string("/dev/video0"));
node_.param("brightness", brightness_, -1); //0-255, -1 "leave alone"
node_.param("contrast", contrast_, -1); //0-255, -1 "leave alone"
node_.param("saturation", saturation_, -1); //0-255, -1 "leave alone"
node_.param("sharpness", sharpness_, -1); //0-255, -1 "leave alone"
// possible values: mmap, read, userptr
node_.param("io_method", io_method_name_, std::string("mmap"));
node_.param("image_width", image_width_, 800);
node_.param("image_height", image_height_, 600);
node_.param("framerate", framerate_, 20);
// possible values: yuyv, uyvy, mjpeg, yuvmono10, rgb24
node_.param("pixel_format", pixel_format_name_, std::string("yuyv"));
// enable/disable autofocus
node_.param("autofocus", autofocus_, false);
node_.param("focus", focus_, -1); //0-255, -1 "leave alone"
// enable/disable autoexposure
node_.param("autoexposure", autoexposure_, true);
node_.param("exposure", exposure_, 100);
node_.param("gain", gain_, -1); //0-100?, -1 "leave alone"
// enable/disable auto white balance temperature
node_.param("auto_white_balance", auto_white_balance_, true);
node_.param("white_balance", white_balance_, 4000);
// load the camera info
node_.param("camera_frame_id", img_.header.frame_id, std::string("head_camera"));
node_.param("camera_name", camera_name_, std::string("head_camera"));
node_.param("camera_info_url", camera_info_url_, std::string(""));
cinfo_.reset(new camera_info_manager::CameraInfoManager(node_, camera_name_, camera_info_url_));
// check for default camera info
if (!cinfo_->isCalibrated())
{
cinfo_->setCameraName(video_device_name_);
sensor_msgs::CameraInfo camera_info;
camera_info.header.frame_id = img_.header.frame_id;
camera_info.width = image_width_;
camera_info.height = image_height_;
cinfo_->setCameraInfo(camera_info);
}
ROS_INFO("Starting '%s' (%s) at %dx%d via %s (%s) at %i FPS", camera_name_.c_str(), video_device_name_.c_str(),
image_width_, image_height_, io_method_name_.c_str(), pixel_format_name_.c_str(), framerate_);
// set the IO method
UsbCam::io_method io_method = UsbCam::io_method_from_string(io_method_name_);
if(io_method == UsbCam::IO_METHOD_UNKNOWN)
{
ROS_FATAL("Unknown IO method '%s'", io_method_name_.c_str());
node_.shutdown();
return;
}
// set the pixel format
UsbCam::pixel_format pixel_format = UsbCam::pixel_format_from_string(pixel_format_name_);
if (pixel_format == UsbCam::PIXEL_FORMAT_UNKNOWN)
{
ROS_FATAL("Unknown pixel format '%s'", pixel_format_name_.c_str());
node_.shutdown();
return;
}
// start the camera
cam_.start(video_device_name_.c_str(), io_method, pixel_format, image_width_,
image_height_, framerate_);
// set camera parameters
if (brightness_ >= 0)
{
cam_.set_v4l_parameter("brightness", brightness_);
}
if (contrast_ >= 0)
{
cam_.set_v4l_parameter("contrast", contrast_);
}
if (saturation_ >= 0)
{
cam_.set_v4l_parameter("saturation", saturation_);
}
if (sharpness_ >= 0)
{
cam_.set_v4l_parameter("sharpness", sharpness_);
}
if (gain_ >= 0)
{
cam_.set_v4l_parameter("gain", gain_);
}
// check auto white balance
if (auto_white_balance_)
{
cam_.set_v4l_parameter("white_balance_temperature_auto", 1);
}
else
{
cam_.set_v4l_parameter("white_balance_temperature_auto", 0);
cam_.set_v4l_parameter("white_balance_temperature", white_balance_);
}
// check auto exposure
if (!autoexposure_)
{
// turn down exposure control (from max of 3)
cam_.set_v4l_parameter("exposure_auto", 1);
// change the exposure level
cam_.set_v4l_parameter("exposure_absolute", exposure_);
}
// check auto focus
if (autofocus_)
{
cam_.set_auto_focus(1);
cam_.set_v4l_parameter("focus_auto", 1);
}
else
{
cam_.set_v4l_parameter("focus_auto", 0);
if (focus_ >= 0)
{
cam_.set_v4l_parameter("focus_absolute", focus_);
}
}
}
/*!
* \brief Gets parameters from the robot_description and configures the powercube_chain.
*/
void getRobotDescriptionParameters()
{
unsigned int DOF = pc_params_->GetDOF();
std::vector<std::string> JointNames = pc_params_->GetJointNames();
/// Get robot_description from ROS parameter server
std::string param_name = "robot_description";
std::string full_param_name;
std::string xml_string;
n_.searchParam(param_name, full_param_name);
if (n_.hasParam(full_param_name))
{
n_.getParam(full_param_name.c_str(), xml_string);
}
else
{
ROS_ERROR("Parameter %s not set, shutting down node...", full_param_name.c_str());
n_.shutdown();
}
if (xml_string.size() == 0)
{
ROS_ERROR("Unable to load robot model from parameter %s",full_param_name.c_str());
n_.shutdown();
}
ROS_DEBUG("%s content\n%s", full_param_name.c_str(), xml_string.c_str());
/// Get urdf model out of robot_description
urdf::Model model;
if (!model.initString(xml_string))
{
ROS_ERROR("Failed to parse urdf file");
n_.shutdown();
}
ROS_DEBUG("Successfully parsed urdf file");
/// Get max velocities out of urdf model
std::vector<double> MaxVelocities(DOF);
for (unsigned int i = 0; i < DOF; i++)
{
MaxVelocities[i] = model.getJoint(JointNames[i].c_str())->limits->velocity;
}
/// Get lower limits out of urdf model
std::vector<double> LowerLimits(DOF);
for (unsigned int i = 0; i < DOF; i++)
{
LowerLimits[i] = model.getJoint(JointNames[i].c_str())->limits->lower;
}
// Get upper limits out of urdf model
std::vector<double> UpperLimits(DOF);
for (unsigned int i = 0; i < DOF; i++)
{
UpperLimits[i] = model.getJoint(JointNames[i].c_str())->limits->upper;
}
/// Get offsets out of urdf model
std::vector<double> Offsets(DOF);
for (unsigned int i = 0; i < DOF; i++)
{
Offsets[i] = model.getJoint(JointNames[i].c_str())->calibration->rising.get()[0];
}
/// Set parameters
pc_params_->SetMaxVel(MaxVelocities);
pc_params_->SetLowerLimits(LowerLimits);
pc_params_->SetUpperLimits(UpperLimits);
pc_params_->SetOffsets(Offsets);
}
/*!
* \brief Gets parameters from the ROS parameter server and configures the powercube_chain.
*/
void getROSParameters()
{
/// get CanModule
std::string CanModule;
if (n_.hasParam("can_module"))
{
n_.getParam("can_module", CanModule);
}
else
{
ROS_ERROR("Parameter can_module not set, shutting down node...");
n_.shutdown();
}
/// get CanDevice
std::string CanDevice;
if (n_.hasParam("can_device"))
{
n_.getParam("can_device", CanDevice);
}
else
{
ROS_ERROR("Parameter can_device not set, shutting down node...");
n_.shutdown();
}
/// get CanBaudrate
int CanBaudrate;
if (n_.hasParam("can_baudrate"))
{
n_.getParam("can_baudrate", CanBaudrate);
}
else
{
ROS_ERROR("Parameter can_baudrate not set, shutting down node...");
n_.shutdown();
}
/// get Modul IDs
XmlRpc::XmlRpcValue ModulIDsXmlRpc;
std::vector<int> ModulIDs;
if (n_.hasParam("modul_ids"))
{
n_.getParam("modul_ids", ModulIDsXmlRpc);
}
else
{
ROS_ERROR("Parameter modul_ids not set, shutting down node...");
n_.shutdown();
}
/// get force_use_movevel
bool UseMoveVel;
if (n_.hasParam("force_use_movevel"))
{
n_.getParam("force_use_movevel", UseMoveVel);
ROS_INFO("Parameter force_use_movevel set, using moveVel");
}
else
{
ROS_INFO("Parameter force_use_movevel not set, using moveStep");
UseMoveVel = false;
}
pc_params_->SetUseMoveVel(UseMoveVel);
/// Resize and assign of values to the ModulIDs
ModulIDs.resize(ModulIDsXmlRpc.size());
for (int i = 0; i < ModulIDsXmlRpc.size(); i++)
{
ModulIDs[i] = (int)ModulIDsXmlRpc[i];
}
/// Initialize parameters
pc_params_->Init(CanModule, CanDevice, CanBaudrate, ModulIDs);
/// Get joint names
XmlRpc::XmlRpcValue JointNamesXmlRpc;
std::vector<std::string> JointNames;
if (n_.hasParam("joint_names"))
{
n_.getParam("joint_names", JointNamesXmlRpc);
}
else
{
ROS_ERROR("Parameter joint_names not set, shutting down node...");
n_.shutdown();
}
/// Resize and assign of values to the JointNames
JointNames.resize(JointNamesXmlRpc.size());
for (int i = 0; i < JointNamesXmlRpc.size(); i++)
{
JointNames[i] = (std::string)JointNamesXmlRpc[i];
}
/// Check dimension with with DOF
if ((int)JointNames.size() != pc_params_->GetDOF())
{
ROS_ERROR("Wrong dimensions of parameter joint_names, shutting down node...");
n_.shutdown();
}
pc_params_->SetJointNames(JointNames);
/// Get max accelerations
XmlRpc::XmlRpcValue MaxAccelerationsXmlRpc;
std::vector<double> MaxAccelerations;
if (n_.hasParam("max_accelerations"))
{
n_.getParam("max_accelerations", MaxAccelerationsXmlRpc);
}
else
{
ROS_ERROR("Parameter max_accelerations not set, shutting down node...");
n_.shutdown();
}
/// Resize and assign of values to the MaxAccelerations
MaxAccelerations.resize(MaxAccelerationsXmlRpc.size());
for (int i = 0; i < MaxAccelerationsXmlRpc.size(); i++)
{
MaxAccelerations[i] = (double)MaxAccelerationsXmlRpc[i];
}
/// Check dimension with with DOF
if ((int)MaxAccelerations.size() != pc_params_->GetDOF())
{
ROS_ERROR("Wrong dimensions of parameter max_accelerations, shutting down node...");
n_.shutdown();
}
pc_params_->SetMaxAcc(MaxAccelerations);
/// Get horizon
double Horizon;
if (n_.hasParam("horizon"))
{
n_.getParam("horizon", Horizon);
}
else
{
/// Horizon in sec
Horizon = 0.05;
ROS_WARN("Parameter horizon not available, setting to default value: %f sec", Horizon);
}
pc_ctrl_->setHorizon(Horizon);
}
//----------------------------------------------------------------------------
~OculusDb()
{
ROS_ASSERT(shutdown_oculusdb());
nh.shutdown();
}
UsbCamNode() :
node_("~")
{
image_transport::ImageTransport it(node_);
image_pub_ = it.advertiseCamera("image_raw", 1);
node_.param("video_device", video_device_name_, std::string("/dev/video0"));
node_.param("io_method", io_method_name_, std::string("mmap")); // possible values: mmap, read, userptr
node_.param("image_width", image_width_, 640);
node_.param("image_height", image_height_, 480);
node_.param("framerate", framerate_, 30);
node_.param("pixel_format", pixel_format_name_, std::string("mjpeg")); // possible values: yuyv, uyvy, mjpeg
node_.param("autofocus", autofocus_, false); // enable/disable autofocus
node_.param("camera_frame_id", img_.header.frame_id, std::string("head_camera"));
node_.param("camera_name", camera_name_, std::string("head_camera"));
node_.param("camera_info_url", camera_info_url_, std::string(""));
cinfo_.reset(new camera_info_manager::CameraInfoManager(node_, camera_name_, camera_info_url_));
ROS_INFO("Camera name: %s", camera_name_.c_str());
ROS_INFO("Camera info url: %s", camera_info_url_.c_str());
ROS_INFO("usb_cam video_device set to [%s]\n", video_device_name_.c_str());
ROS_INFO("usb_cam io_method set to [%s]\n", io_method_name_.c_str());
ROS_INFO("usb_cam image_width set to [%d]\n", image_width_);
ROS_INFO("usb_cam image_height set to [%d]\n", image_height_);
ROS_INFO("usb_cam pixel_format set to [%s]\n", pixel_format_name_.c_str());
ROS_INFO("usb_cam auto_focus set to [%d]\n", autofocus_);
usb_cam_io_method io_method;
if(io_method_name_ == "mmap")
io_method = IO_METHOD_MMAP;
else if(io_method_name_ == "read")
io_method = IO_METHOD_READ;
else if(io_method_name_ == "userptr")
io_method = IO_METHOD_USERPTR;
else {
ROS_FATAL("Unknown io method.");
node_.shutdown();
return;
}
usb_cam_pixel_format pixel_format;
if(pixel_format_name_ == "yuyv")
pixel_format = PIXEL_FORMAT_YUYV;
else if(pixel_format_name_ == "uyvy")
pixel_format = PIXEL_FORMAT_UYVY;
else if(pixel_format_name_ == "mjpeg") {
pixel_format = PIXEL_FORMAT_MJPEG;
}
else {
ROS_FATAL("Unknown pixel format.");
node_.shutdown();
return;
}
camera_image_ = usb_cam_camera_start(video_device_name_.c_str(),
io_method,
pixel_format,
image_width_,
image_height_,
framerate_);
if(autofocus_) {
usb_cam_camera_set_auto_focus(1);
}
next_time_ = ros::Time::now();
count_ = 0;
}
/*!
* \brief Gets parameters from the ROS parameter server and configures the powercube_chain.
*/
void getROSParameters()
{
// get CanModule
std::string CanModule;
if (n_.hasParam("can_module"))
{
n_.getParam("can_module", CanModule);
}
else
{
ROS_ERROR("Parameter can_module not set, shutting down node...");
n_.shutdown();
}
// get CanDevice
std::string CanDevice;
if (n_.hasParam("can_device"))
{
n_.getParam("can_device", CanDevice);
}
else
{
ROS_ERROR("Parameter can_device not set, shutting down node...");
n_.shutdown();
}
// get CanBaudrate
int CanBaudrate;
if (n_.hasParam("can_baudrate"))
{
n_.getParam("can_baudrate", CanBaudrate);
}
else
{
ROS_ERROR("Parameter can_baudrate not set, shutting down node...");
n_.shutdown();
}
// get modul ids
XmlRpc::XmlRpcValue ModulIDsXmlRpc;
std::vector<int> ModulIDs;
if (n_.hasParam("modul_ids"))
{
n_.getParam("modul_ids", ModulIDsXmlRpc);
}
else
{
ROS_ERROR("Parameter modul_ids not set, shutting down node...");
n_.shutdown();
}
ModulIDs.resize(ModulIDsXmlRpc.size());
for (int i = 0; i < ModulIDsXmlRpc.size(); i++)
{
ModulIDs[i] = (int)ModulIDsXmlRpc[i];
}
// init parameters
pc_params_->Init(CanModule, CanDevice, CanBaudrate, ModulIDs);
// get joint names
XmlRpc::XmlRpcValue JointNamesXmlRpc;
std::vector<std::string> JointNames;
if (n_.hasParam("joint_names"))
{
n_.getParam("joint_names", JointNamesXmlRpc);
}
else
{
ROS_ERROR("Parameter joint_names not set, shutting down node...");
n_.shutdown();
}
JointNames.resize(JointNamesXmlRpc.size());
for (int i = 0; i < JointNamesXmlRpc.size(); i++)
{
JointNames[i] = (std::string)JointNamesXmlRpc[i];
}
// check dimension with with DOF
if ((int)JointNames.size() != pc_params_->GetDOF())
{
ROS_ERROR("Wrong dimensions of parameter joint_names, shutting down node...");
n_.shutdown();
}
pc_params_->SetJointNames(JointNames);
// get max accelerations
XmlRpc::XmlRpcValue MaxAccelerationsXmlRpc;
std::vector<double> MaxAccelerations;
if (n_.hasParam("max_accelerations"))
{
n_.getParam("max_accelerations", MaxAccelerationsXmlRpc);
}
else
{
ROS_ERROR("Parameter max_accelerations not set, shutting down node...");
n_.shutdown();
}
MaxAccelerations.resize(MaxAccelerationsXmlRpc.size());
for (int i = 0; i < MaxAccelerationsXmlRpc.size(); i++)
{
MaxAccelerations[i] = (double)MaxAccelerationsXmlRpc[i];
}
// check dimension with with DOF
if ((int)MaxAccelerations.size() != pc_params_->GetDOF())
{
ROS_ERROR("Wrong dimensions of parameter max_accelerations, shutting down node...");
n_.shutdown();
}
pc_params_->SetMaxAcc(MaxAccelerations);
// get horizon
double Horizon;
if (n_.hasParam("horizon"))
{
n_.getParam("horizon", Horizon);
}
else
{
Horizon = 0.025; //Hz
ROS_WARN("Parameter horizon not available, setting to default value: %f sec", Horizon);
}
pc_ctrl_->setHorizon(Horizon);
}
/*!
* \brief Initializes node to get parameters, subscribe and publish to topics.
*/
bool init()
{
// initialize member variables
isInitialized_ = false;
isDSAInitialized_ = false;
hasNewGoal_ = false;
// implementation of topics to publish
topicPub_JointState_ = nh_.advertise<sensor_msgs::JointState>("/joint_states", 1);
topicPub_ControllerState_ = nh_.advertise<pr2_controllers_msgs::JointTrajectoryControllerState>("state", 1);
topicPub_TactileSensor_ = nh_.advertise<schunk_sdh::TactileSensor>("tactile_data", 1);
// pointer to sdh
sdh_ = new SDH::cSDH(false, false, 0); //(_use_radians=false, bool _use_fahrenheit=false, int _debug_level=0)
// implementation of service servers
srvServer_Init_ = nh_.advertiseService("init", &SdhNode::srvCallback_Init, this);
srvServer_Stop_ = nh_.advertiseService("stop", &SdhNode::srvCallback_Stop, this);
srvServer_Recover_ = nh_.advertiseService("recover", &SdhNode::srvCallback_Recover, this);
srvServer_SetOperationMode_ = nh_.advertiseService("set_operation_mode", &SdhNode::srvCallback_SetOperationMode, this);
// getting hardware parameters from parameter server
nh_.param("sdhdevicetype", sdhdevicetype_, std::string("PCAN"));
nh_.param("sdhdevicestring", sdhdevicestring_, std::string("/dev/pcan0"));
nh_.param("sdhdevicenum", sdhdevicenum_, 0);
nh_.param("dsadevicestring", dsadevicestring_, std::string("/dev/ttyS0"));
nh_.param("dsadevicenum", dsadevicenum_, 0);
nh_.param("baudrate", baudrate_, 1000000);
nh_.param("timeout", timeout_, (double)0.04);
nh_.param("id_read", id_read_, 43);
nh_.param("id_write", id_write_, 42);
// get joint_names from parameter server
ROS_INFO("getting joint_names from parameter server");
XmlRpc::XmlRpcValue joint_names_param;
if (nh_.hasParam("joint_names"))
{
nh_.getParam("joint_names", joint_names_param);
}
else
{
ROS_ERROR("Parameter joint_names not set, shutting down node...");
nh_.shutdown();
return false;
}
DOF_ = joint_names_param.size();
joint_names_.resize(DOF_);
for (int i = 0; i<DOF_; i++ )
{
joint_names_[i] = (std::string)joint_names_param[i];
}
std::cout << "joint_names = " << joint_names_param << std::endl;
// define axes to send to sdh
axes_.resize(DOF_);
for(int i=0; i<DOF_; i++)
{
axes_[i] = i;
}
ROS_INFO("DOF = %d",DOF_);
state_.resize(axes_.size());
return true;
}