C++ NodeHandle::shutdown方法代码示例

  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());
//.........这里部分代码省略.........

		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
//.........这里部分代码省略.........

  /*!
   * \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++)
    {
//.........这里部分代码省略.........

 //----------------------------------------------------------------------------
 ~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());
//.........这里部分代码省略.........

		/*!
		* \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;
		}