/*!
   * \brief Publishes the state of the powercube_chain as ros messages.
   *
   * Published to "/joint_states" as "sensor_msgs/JointState"
   * Published to "state" as "pr2_controllers_msgs/JointTrajectoryState"
   */
  void publishState(bool update=true)
  {
	  if (initialized_)
	  {
		  ROS_DEBUG("publish state");

		  if(update)
			{pc_ctrl_->updateStates();}

		  sensor_msgs::JointState joint_state_msg;
		  joint_state_msg.header.stamp = ros::Time::now();
		  joint_state_msg.name = pc_params_->GetJointNames();
		  joint_state_msg.position = pc_ctrl_->getPositions();
		  joint_state_msg.velocity = pc_ctrl_->getVelocities();
		  joint_state_msg.effort.resize(pc_params_->GetDOF());

		  pr2_controllers_msgs::JointTrajectoryControllerState controller_state_msg;
		  controller_state_msg.header.stamp = joint_state_msg.header.stamp;
		  controller_state_msg.joint_names = pc_params_->GetJointNames();
		  controller_state_msg.actual.positions = pc_ctrl_->getPositions();
		  controller_state_msg.actual.velocities = pc_ctrl_->getVelocities();
		  controller_state_msg.actual.accelerations = pc_ctrl_->getAccelerations();

		  std_msgs::String opmode_msg;
		  opmode_msg.data = "velocity";

		  /// publishing joint and controller states on topic
		  topicPub_JointState_.publish(joint_state_msg);
		  topicPub_ControllerState_.publish(controller_state_msg);
		  topicPub_OperationMode_.publish(opmode_msg);

		  last_publish_time_ = joint_state_msg.header.stamp;

	  }

	// check status of PowerCube chain
	if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK) { error_ = true; }
	
		// check status of PowerCube chain 
		if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK) 
		{ 	
			error_ = true;
		} 
		else 
		{
			error_ = false;
		} 

    // publishing diagnotic messages
    diagnostic_msgs::DiagnosticArray diagnostics;
    diagnostics.status.resize(1);

    // set data to diagnostics
    if(error_)
    {
      diagnostics.status[0].level = 2;
      diagnostics.status[0].name = n_.getNamespace();;
      diagnostics.status[0].message =  pc_ctrl_->getErrorMessage();
    }
    else
    {
      if (initialized_)
      {
        diagnostics.status[0].level = 0;
        diagnostics.status[0].name = n_.getNamespace(); //"schunk_powercube_chain";
        diagnostics.status[0].message = "powercubechain initialized and running";
      }
      else
      {
        diagnostics.status[0].level = 1;
        diagnostics.status[0].name = n_.getNamespace(); //"schunk_powercube_chain";
        diagnostics.status[0].message = "powercubechain not initialized";
      }
    }
    // publish diagnostic message
    topicPub_Diagnostic_.publish(diagnostics);
  }
  /*!
   * \brief Executes the callback from the command_vel topic.
   *
   * Set the current velocity target.
   * \param msg JointVelocities
   */
  void topicCallback_CommandVel(const brics_actuator::JointVelocities::ConstPtr& msg)
  {
	  ROS_DEBUG("Received new velocity command");
	  if (!initialized_)
	  {	
			ROS_WARN("Skipping command: powercubes not initialized");
			publishState(false);
			return; 
		}
		
		if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK)
		{
			publishState(false);
			return; 
		}

 
	  PowerCubeCtrl::PC_CTRL_STATUS status;
	  std::vector<std::string> errorMessages;
	  pc_ctrl_->getStatus(status, errorMessages);

	  /// ToDo: don't rely on position of joint names, but merge them (check between msg.joint_uri and member variable JointStates)

	  unsigned int DOF = pc_params_->GetDOF();
	  std::vector<std::string> jointNames = pc_params_->GetJointNames();
	  std::vector<double> cmd_vel(DOF);
	  std::string unit = "rad";

	  /// check dimensions
	  if (msg->velocities.size() != DOF)
	  {
		  ROS_ERROR("Skipping command: Commanded velocities and DOF are not same dimension.");
		  return;
	  }

	  /// parse velocities
	  for (unsigned int i = 0; i < DOF; i++)
	  {
		  /// check joint name
		  if (msg->velocities[i].joint_uri != jointNames[i])
		  {
			  ROS_ERROR("Skipping command: Received joint name %s doesn't match expected joint name %s for joint %d.",msg->velocities[i].joint_uri.c_str(),jointNames[i].c_str(),i);
			  return;
		  }

		  /// check unit
		  if (msg->velocities[i].unit != unit)
		  {
			  ROS_ERROR("Skipping command: Received unit %s doesn't match expected unit %s.",msg->velocities[i].unit.c_str(),unit.c_str());
			  return;
		  }

		  /// if all checks are successful, parse the velocity value for this joint
		  ROS_DEBUG("Parsing velocity %f for joint %s",msg->velocities[i].value,jointNames[i].c_str());
		  cmd_vel[i] = msg->velocities[i].value;
	  }
		
	  /// command velocities to powercubes
	  if (!pc_ctrl_->MoveVel(cmd_vel))
	  {
     	error_ = true;
			error_msg_ = pc_ctrl_->getErrorMessage();
		  ROS_ERROR("Skipping command: %s",pc_ctrl_->getErrorMessage().c_str());
		  return;
	  }

	  ROS_DEBUG("Executed velocity command");
	 
	  publishState(false);
  }
  /*!
   * \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);
  }
  /*!
   * \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();
    }
    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);
  }