/*!
		* \brief Executes the service callback for recover.
		*
		* Recovers the driver after an emergency stop.
		* \param req Service request
		* \param res Service response
		*/
		bool srvCallback_Recover(	cob_srvs::Trigger::Request &req,
									cob_srvs::Trigger::Response &res )
		{
			if (isInitialized_ == true)
			{
		   		ROS_INFO("Recovering powercubes");
		
				// stopping all arm movements
				if (PCube_->Stop())
				{
					ROS_INFO("Recovering powercubes succesfull");
					res.success.data = true;
				}
				else
				{
					ROS_ERROR("Recovering powercubes not succesfull. error: %s", PCube_->getErrorMessage().c_str());
					res.success.data = false;
					res.error_message.data = PCube_->getErrorMessage();
				}
			}
			else
			{
				ROS_ERROR("...powercubes already recovered...");
				res.success.data = false;
				res.error_message.data = "powercubes already recovered";
			}

			return true;
		}
		/*!
		* \brief Executes the service callback for init.
		*
		* Connects to the hardware and initialized it.
		* \param req Service request
		* \param res Service response
		*/
		bool srvCallback_Init(	cob_srvs::Trigger::Request &req,
								cob_srvs::Trigger::Response &res )
		{
			if (isInitialized_ == false)
			{
				ROS_INFO("...initializing powercubes...");
				// init powercubes 
				if (PCube_->Init(PCubeParams_))
				{
					ROS_INFO("Initializing succesfull");
					isInitialized_ = true;
					res.success.data = true;
					res.error_message.data = "success";
				}
				else
				{
					ROS_ERROR("Initializing powercubes not succesfull. error: %s", PCube_->getErrorMessage().c_str());
					res.success.data = false;
					res.error_message.data = PCube_->getErrorMessage();
				}
			}
			else
			{
				ROS_ERROR("...powercubes already initialized...");		        
				res.success.data = false;
				res.error_message.data = "powercubes already initialized";
			}

			return true;
		}
  /*!
   * \brief Executes the service callback for recover.
   *
   * Recovers the driver after an emergency stop.
   * \param req Service request
   * \param res Service response
   */
  bool srvCallback_Recover(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res)
  {
	  ROS_INFO("Recovering powercubes...");
	  if (initialized_)
	  {
		  /// stopping all arm movements
		  if (pc_ctrl_->Recover())
		  {
        		error_ = false;
        		error_msg_ = "";
			res.success.data = true;
			ROS_INFO("...recovering powercubes successful.");
		  }
		  else
		  {
			res.success.data = false;
        		error_ = true;
        		error_msg_ = pc_ctrl_->getErrorMessage();
			res.error_message.data = pc_ctrl_->getErrorMessage();
			ROS_ERROR("...recovering powercubes not successful. error: %s", res.error_message.data.c_str());
		  }
	  }
	  else
	  {
		  res.success.data = false;
		  res.error_message.data = "powercubes not initialized";
		  ROS_ERROR("...recovering powercubes not successful. error: %s",res.error_message.data.c_str());
	  }

	  return true;
  }
  /*!
   * \brief Executes the service callback for init.
   *
   * Connects to the hardware and initialized it.
   * \param req Service request
   * \param res Service response
   */
  bool srvCallback_Init(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res)
  {
	  if (!initialized_)
	  {
		  ROS_INFO("Initializing powercubes...");

		  /// initialize powercubes
		  if (pc_ctrl_->Init(pc_params_))
		  {
			  initialized_ = true;
			  res.success.data = true;
			  ROS_INFO("...initializing powercubes successful");
		  }

		  else
		  {
        	  error_ = true;
         	  error_msg_ = pc_ctrl_->getErrorMessage();
			  res.success.data = false;
			  res.error_message.data = pc_ctrl_->getErrorMessage();
			  ROS_INFO("...initializing powercubes not successful. error: %s", res.error_message.data.c_str());
		  }
	  }

	  else
	  {
		  res.success.data = true;
		  res.error_message.data = "powercubes already initialized";
		  ROS_WARN("...initializing powercubes not successful. error: %s",res.error_message.data.c_str());
	  }

	  return true;
  }
		/*!
		* \brief Routine for updating command to the powercubes.
		*
		* Depending on the operation mode (position/velocity) either position or velocity goals are sent to the hardware.
		*/
		void updatePCubeCommands()
		{
			if (isInitialized_ == true)
			{
				std::string operationMode;
				n_.getParam("OperationMode", operationMode);
				if (operationMode == "position")
				{
					ROS_DEBUG("moving powercubes in position mode");
					if (PCube_->statusMoving() == false)
					{
						//feedback_.isMoving = false;
			
						ROS_DEBUG("next point is %d from %d",traj_point_nr_,traj_.points.size());
			
						if (traj_point_nr_ < traj_.points.size())
						{
							// if powercubes are not moving and not reached last point of trajectory, then send new target point
							ROS_DEBUG("...moving to trajectory point[%d]",traj_point_nr_);
							traj_point_ = traj_.points[traj_point_nr_];
							PCube_->MoveJointSpaceSync(traj_point_.positions);
							traj_point_nr_++;
							//feedback_.isMoving = true;
							//feedback_.pointNr = traj_point_nr;
							//as_.publishFeedback(feedback_);
						}
						else
						{
							ROS_DEBUG("...reached end of trajectory");
							finished_ = true;
						}
					}
					else
					{
						ROS_DEBUG("...powercubes still moving to point[%d]",traj_point_nr_);
					}
				}
				else if (operationMode == "velocity")
				{
					ROS_DEBUG("moving powercubes in velocity mode");
					if(newvel_)
					{	
						ROS_INFO("MoveVel Call");
						PCube_->MoveVel(cmd_vel_);
						newvel_ = false;
					}
				}
				else
				{
					ROS_ERROR("powercubes neither in position nor in velocity mode. OperationMode = [%s]", operationMode.c_str());
				}
			}
			else
			{
				ROS_DEBUG("powercubes not initialized");
			}
		}
		/*!
		* \brief Routine for publishing joint_states.
		*
		* Gets current positions and velocities from the hardware and publishes them as joint_states.
		*/
		void publishJointState()
		{
			if (isInitialized_ == true)
			{
				// create joint_state message
				int DOF = ModIds_param_.size();
				std::vector<double> ActualPos;
				std::vector<double> ActualVel;
				ActualPos.resize(DOF);
				ActualVel.resize(DOF);

				PCube_->getConfig(ActualPos);
				PCube_->getJointVelocities(ActualVel);

				sensor_msgs::JointState msg;
				msg.header.stamp = ros::Time::now();
				msg.name.resize(DOF);
				msg.position.resize(DOF);
				msg.velocity.resize(DOF);

				msg.name = JointNames_;

				for (int i = 0; i<DOF; i++ )
				{
					msg.position[i] = ActualPos[i];
					msg.velocity[i] = ActualVel[i];
					//std::cout << "Joint " << msg.name[i] <<": pos="<<  msg.position[i] << "vel=" << msg.velocity[i] << std::endl;
				}
		
				// publish message
				topicPub_JointState_.publish(msg);

				pr2_controllers_msgs::JointTrajectoryControllerState controllermsg;
				controllermsg.header.stamp = ros::Time::now();
				controllermsg.joint_names.resize(DOF);
				controllermsg.actual.positions.resize(DOF);
				controllermsg.actual.velocities.resize(DOF);

				controllermsg.joint_names = JointNames_;

				for (int i = 0; i<DOF; i++ )
				{
					controllermsg.actual.positions[i] = ActualPos[i];
					controllermsg.actual.velocities[i] = ActualVel[i];
					//std::cout << "Joint " << msg.name[i] <<": pos="<<  msg.position[i] << "vel=" << msg.velocity[i] << std::endl;
				}
				topicPub_ControllerState_.publish(controllermsg);

			}
		}
  /*!
   * \brief Executes the service callback for stop.
   *
   * Stops all hardware movements.
   * \param req Service request
   * \param res Service response
   */
  bool srvCallback_Stop(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res)
  {
    ROS_INFO("Stopping powercubes...");

    // stop powercubes
    if (pc_ctrl_->Stop())
    {
      res.success.data = true;
      ROS_INFO("...stopping powercubes successful.");
    }
    else
    {
      res.success.data = false;
      res.error_message.data = pc_ctrl_->getErrorMessage();
      ROS_ERROR("...stopping powercubes not successful. error: %s", res.error_message.data.c_str());
    }
    return true;
  }
  bool srvCallback_Stop(cob3_srvs::Stop::Request &req,
                        cob3_srvs::Stop::Response &res )
  {
 	    ROS_INFO("Stopping powercubes");
  
      // stopping all arm movements
      if (PCube->Stop())
      {
      	ROS_INFO("Stopping powercubes succesfull");
      	res.success = 0; // 0 = true, else = false
      }
      else
      {
      	ROS_ERROR("Stopping powercubes not succesfull. error: %s", PCube->getErrorMessage().c_str());
      	res.success = 1; // 0 = true, else = false
      	res.errorMessage.data = PCube->getErrorMessage();
      }
      return true;
  }
 // topic callback functions 
 // function will be called when a new message arrives on a topic
 void topicCallback_JointCommand(const cob3_msgs::JointCommand::ConstPtr& msg)
 {
     std::string operationMode;
     n.getParam("OperationMode", operationMode);
     if (operationMode == "position")
     {
         ROS_INFO("moving powercubes in position mode");
         //TODO PowerCubeCtrl
         PCube->MoveJointSpaceSync(msg->positions);
     }
     else if (operationMode == "velocity")
     {
     	ROS_INFO("moving powercubes in velocity mode");
         //TODO PowerCubeCtrl
         PCube->MoveVel(msg->velocities);  
     }
     else
     {
         ROS_ERROR("powercubes neither in position nor in velocity mode. OperationMode = [%s]", operationMode.c_str());
     }
 }
  /*!
   * \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()
  {
    if (initialized_)
    {
      ROS_INFO("publish state");

      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();

      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();

      topicPub_JointState_.publish(joint_state_msg);
      topicPub_ControllerState_.publish(controller_state_msg);

      last_publish_time_ = ros::Time::now();
    }
  }
		/*!
		* \brief Executes the service callback for stop.
		*
		* Stops all hardware movements.
		* \param req Service request
		* \param res Service response
		*/
		bool srvCallback_Stop(	cob_srvs::Trigger::Request &req,
								cob_srvs::Trigger::Response &res )
		{
			ROS_INFO("Stopping powercubes");
			newvel_ = false;
	
			// set current trajectory to be finished
			traj_point_nr_ = traj_.points.size();
	
			// stopping all arm movements
			if (PCube_->Stop())
			{
				ROS_INFO("Stopping powercubes succesfull");
				res.success.data = true;
			}
			else
			{
				ROS_ERROR("Stopping powercubes not succesfull. error: %s", PCube_->getErrorMessage().c_str());
				res.success.data = false;
				res.error_message.data = PCube_->getErrorMessage();
			}
			return true;
		}
		/*!
		* \brief Executes the callback from the actionlib.
		*
		* Set the current goal to aborted after receiving a new goal and write new goal to a member variable. Wait for the goal to finish and set actionlib status to succeeded.
		* \param goal JointTrajectoryGoal
		*/
		void executeCB(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal)
		{
			ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size());
			if (!isInitialized_)
			{
				ROS_ERROR("%s: Rejected, powercubes not initialized", action_name_.c_str());
				as_.setAborted();
				return;
			}
			// saving goal into local variables
			traj_ = goal->trajectory;
			traj_point_nr_ = 0;
			traj_point_ = traj_.points[traj_point_nr_];
			finished_ = false;
			
			// stoping arm to prepare for new trajectory
			std::vector<double> VelZero;
			VelZero.resize(ModIds_param_.size());
			PCube_->MoveVel(VelZero);

			// check that preempt has not been requested by the client
			if (as_.isPreemptRequested())
			{
				ROS_INFO("%s: Preempted", action_name_.c_str());
				// set the action state to preempted
				as_.setPreempted();
			}
			
			usleep(500000); // needed sleep until powercubes starts to change status from idle to moving
			
			while(finished_ == false)
			{
				if (as_.isNewGoalAvailable())
				{
					ROS_WARN("%s: Aborted", action_name_.c_str());
					as_.setAborted();
					return;
				}
		   		usleep(10000);
				//feedback_ = 
				//as_.send feedback_
			}

			// set the action state to succeed			
			//result_.result.data = "executing trajectory";
			ROS_INFO("%s: Succeeded", action_name_.c_str());
			// set the action state to succeeded
			as_.setSucceeded(result_);
		}
 // service callback functions
 // function will be called when a service is querried
 bool srvCallback_Init(cob3_srvs::Init::Request &req,
                       cob3_srvs::Init::Response &res )
 {
 	ROS_INFO("Initializing powercubes");
   	
   	// init powercubes 
   	//TODO: make iniFilepath as an argument
   	//TODO: read iniFile into ros prarameters
     if (PCube->Init("include/iniFile.txt")) 
     {
     	ROS_INFO("Initializing succesfull");
     	res.success = 0; // 0 = true, else = false
     }
     else
     {
     	ROS_ERROR("Initializing powercubes not succesfull. error: %s", PCube->getErrorMessage().c_str());
     	res.success = 1; // 0 = true, else = false
     	res.errorMessage.data = PCube->getErrorMessage();
     	return true;
     }
     
     // homing powercubes
     if (PCube->doHoming())
     {
     	ROS_INFO("Homing powercubes succesfull");
     	res.success = 0; // 0 = true, else = false
     }
     else
     {
     	ROS_ERROR("Homing powercubes not succesfull. error: %s", PCube->getErrorMessage().c_str());
     	res.success = 1; // 0 = true, else = false
     	res.errorMessage.data = PCube->getErrorMessage();
     	return true;
     }
     return true;
 }
  /*!
   * \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 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 Destructor for PowercubeChainNode class.
		*/
		~PowercubeChainNode()
		{
			bool closed = PCube_->Close();
			if (closed) ROS_INFO("PowerCube Device closed!");
			if (sem_can_available) sem_close(can_sem);
		}
  /*!
   * \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);
  }
  /// Destructor
  ~PowerCubeChainNode()
  {
	  bool closed = pc_ctrl_->Close();
	  if (closed)
      ROS_INFO("PowerCube Device closed!");
  }
		/*!
		* \brief Routine for publishing joint_states.
		*
		* Gets current positions and velocities from the hardware and publishes them as joint_states.
		*/
		void publishJointState()
		{
		  updater_.update();
			if (isInitialized_ == true)
			{
				// publish joint state message
				int DOF = ModIds_param_.size();
				std::vector<double> ActualPos;
				std::vector<double> ActualVel;
				ActualPos.resize(DOF);
				ActualVel.resize(DOF);

				lock_semaphore(can_sem);
				int success = PCube_->getConfig(ActualPos);
				if (!success) return;
				PCube_->getJointVelocities(ActualVel);
				unlock_semaphore(can_sem);

				sensor_msgs::JointState msg;
				msg.header.stamp = ros::Time::now();
				msg.name.resize(DOF);
				msg.position.resize(DOF);
				msg.velocity.resize(DOF);

				msg.name = JointNames_;

				for (int i = 0; i<DOF; i++ )
				{
					msg.position[i] = ActualPos[i];
					msg.velocity[i] = ActualVel[i];
					//std::cout << "Joint " << msg.name[i] <<": pos="<<  msg.position[i] << "vel=" << msg.velocity[i] << std::endl;
				}

				topicPub_JointState_.publish(msg);

				// publish controller state message
				pr2_controllers_msgs::JointTrajectoryControllerState controllermsg;
				controllermsg.header.stamp = ros::Time::now();
				controllermsg.joint_names.resize(DOF);
				controllermsg.desired.positions.resize(DOF);
				controllermsg.desired.velocities.resize(DOF);
				controllermsg.actual.positions.resize(DOF);
				controllermsg.actual.velocities.resize(DOF);
				controllermsg.error.positions.resize(DOF);
				controllermsg.error.velocities.resize(DOF);

				controllermsg.joint_names = JointNames_;

				for (int i = 0; i<DOF; i++ )
				{
					//std::cout << "Joint " << msg.name[i] <<": pos="<<  msg.position[i] << "vel=" << msg.velocity[i] << std::endl;
					
					if (traj_point_.positions.size() != 0)
						controllermsg.desired.positions[i] = traj_point_.positions[i];
					else
						controllermsg.desired.positions[i] = 0.0;
					controllermsg.desired.velocities[i] = 0.0;
					
					controllermsg.actual.positions[i] = ActualPos[i];
					controllermsg.actual.velocities[i] = ActualVel[i];
					
					controllermsg.error.positions[i] = controllermsg.desired.positions[i] - controllermsg.actual.positions[i];
					controllermsg.error.velocities[i] = controllermsg.desired.velocities[i] - controllermsg.actual.velocities[i];
				}
				topicPub_ControllerState_.publish(controllermsg);

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