void GroupCommandController::commandCB(const std_msgs::Float64MultiArrayConstPtr& msg)
    {
		 #if TRACE_GroupCommandController_ACTIVATED
			ROS_INFO("GroupCommandController: Start commandCB of robot %s!",robot_namespace_.c_str());
		 #endif
		 
		
	     if(msg->data.size()!=joint_handles_.size())
	     { 
	       ROS_ERROR_STREAM("GroupCommandController: Dimension (of robot " << robot_namespace_.c_str() << ") of command (" << msg->data.size() << ") does not match number of joints (" << joint_handles_.size() << ")! Not executing!");
	       cmd_flag_ = 0;
	       return; 
	     }
	     
	     // clear buffers for initial values
	     commands_buffer_.clear();
	     goal_buffer_.clear();
	     goal_factor_.clear();
	     
	     for (size_t i=0; i<joint_handles_.size(); ++i)
	     {
			commands_buffer_.push_back(msg->data[i]);
			
			// set the factor of increment/decrement depending of the current joint value and the desired one
			if (joint_handles_[i].getPosition() > msg->data[i]) 
				goal_factor_.push_back(-1);
			else 
				goal_factor_.push_back(1);
		 }
		 
	     cmd_flag_ = 1; // set this flag to 1 to run the update method
	     
	     #if TRACE_GroupCommandController_ACTIVATED
			ROS_INFO("GroupCommandController: Finish commandCB of robot %s !",robot_namespace_.c_str());
			ROS_INFO("GroupCommandController: of robot %s -> j0=%f, j1=%f, j2=%f, j3=%f, j4=%f, j5=%f, j6=%f",robot_namespace_.c_str(),commands_buffer_[0],commands_buffer_[1],commands_buffer_[2],commands_buffer_[3],commands_buffer_[4],commands_buffer_[5],commands_buffer_[6]);

		 #endif
		 
	}

void
KalmanDetectionFilter::drawFilterStates()
{
    if(!current_filter_)
        return;
    geometry_msgs::PointStamped temp_msg, temp_msg_base_link;
    temp_msg.header.frame_id = "odom";
    temp_msg.header.stamp = ros::Time(0);

    cv::Mat img = cv::Mat::zeros(500, 500, CV_8UC3);
    float px_per_meter = 50.0;
    float offset = 250;
    temp_msg.point.x = current_filter_->statePost.at<float>(0);
    temp_msg.point.y = current_filter_->statePost.at<float>(1);
    temp_msg.point.z = 0.0;

    if( listener_.canTransform( "base_link", temp_msg.header.frame_id, temp_msg.header.stamp))
    {
        listener_.transformPoint("base_link",temp_msg,temp_msg_base_link);
    }
    else
    {
        ROS_ERROR_STREAM("cannot transform filter from odom to base_link");
        return;
    }

    cv::Point mean(temp_msg_base_link.point.x * px_per_meter,
            temp_msg_base_link.point.y * px_per_meter);
    float rad_x = current_filter_->errorCovPost.at<float>(0,0) * px_per_meter;
    float rad_y = current_filter_->errorCovPost.at<float>(1,1) * px_per_meter;
    cv::circle(img, mean+cv::Point(0,offset), 5, cv::Scalar(255,0,0));
    cv::ellipse(img, mean+cv::Point(0,offset), cv::Size(rad_x, rad_y), 0, 0, 360, cv::Scalar(0,255,0));

    //printFilterState();
    std_msgs::Header header;
    sensor_msgs::ImagePtr debug_img_msg = cv_bridge::CvImage(header,"rgb8",img).toImageMsg();
    pub_debug_img.publish(debug_img_msg);
}

bool MCU::rcvMCUMsg(double & angle)
{
    bool result = false;
    char b0, b1, b2, b3, b4, b5;

    if (!serialPort.get(b0)) return false;
    if (b0 != MCU_RESP_START_BYTE) return false;
    if (!serialPort.get(b1)) return false;
    if (!serialPort.get(b2)) return false;
    if (!serialPort.get(b3)) return false;
    if (!serialPort.get(b4)) return false;
    if (!serialPort.get(b5)) return false;

    // Perform checksum
    char checksum;
    checksum = b0 ^ b1 ^ b2 ^ b3  ^ b4;
    if (checksum == b5)
    {
        int stepPosition = (b1 << 24) + (b2 << 16) + (b3 << 8) + (b4 & 0xFF);
        angle = static_cast<double>(stepPosition) / STEPS_PER_REV * 360;

        // ROS_INFO_STREAM("MCU::rcvMCUMsg: Received current angle: " << currAngle << ", stepPosition: " << stepPosition << "\n"
        //     << "b0 = " << std::hex << "0x" << static_cast<int>(b0) << "\n"
        //     << "b1 = " << std::hex << "0x" << static_cast<int>(b1) << "\n"
        //     << "b2 = " << std::hex << "0x" << static_cast<int>(b2) << "\n"
        //     << "b3 = " << std::hex << "0x" << static_cast<int>(b3) << "\n"
        //     << "b4 = " << std::hex << "0x" << static_cast<int>(b4) << "\n"
        //     << "b5 = " << std::hex << "0x" << static_cast<int>(b5));
        result = true;
    }
    else
    {
        ROS_ERROR_STREAM("MCU::rcvMCUMsg: Bad checksum of 0x" << std::hex << checksum << ", expected 0x" << b5);
        result = false;
    }

    return result;
}

bool ROSRuntimeUtils::store_tf_broadcasters(std::string &package_path, std::string &file_name)
{
  std::string filepath = package_path+file_name;
  std::ofstream output_file(filepath.c_str(), std::ios::out);// | std::ios::app);
  if (output_file.is_open())
  {
    ROS_INFO_STREAM("Storing results in: "<<filepath);
  }
  else
  {
    ROS_ERROR_STREAM("Unable to open file");
    return false;
  }
  output_file << "<launch>";
  //have calibrated transforms
  double roll, pitch, yaw, x_position, y_position, z_position;
  tf::Vector3 origin;
  tf::Matrix3x3 orientation;
  for (int i=0; i<calibrated_transforms_.size(); i++)
  {
    origin = calibrated_transforms_.at(i).getOrigin();
    x_position=origin.getX();
    y_position=origin.getY();
    z_position=origin.getZ();
    orientation = calibrated_transforms_.at(i).getBasis();
    orientation.getEulerYPR(yaw, pitch, roll);
    output_file<<"\n";
    output_file<<" <node pkg=\"tf\" type=\"static_transform_publisher\" name=\"world_to_camera"<<i<<"\" args=\"";
    //tranform publisher launch files requires x y z yaw pitch roll
    output_file<<x_position<< ' '<<y_position<< ' '<<z_position<< ' '<<yaw<< ' '<<pitch<< ' '<<roll ;
    output_file<<" "<<world_frame_;
    output_file<<" "<<camera_intermediate_frame_[i];
    output_file<<" 100\" />";
  }
  output_file<<"\n";
  output_file << "</launch>";
  return true;
}

std::vector<urdf_traverser::JointPtr>
urdf_traverser::getChain(const LinkConstPtr& from_link, const LinkConstPtr& to_link)
{
    assert(from_link);
    assert(to_link);
    std::vector<JointPtr> chain;

    if (to_link->name == from_link->name) return chain;

    LinkConstPtr curr = to_link;
    LinkConstPtr pl = to_link->getParent();

    while (curr && (curr->name != from_link->name))
    {
        // ROS_INFO_STREAM("Chain: "<<curr->name);
        JointPtr pj = curr->parent_joint;
        if (!pj)
        {
            ROS_ERROR("UrdfTraverser: End of chain at link '%s'", curr->name.c_str());
            return std::vector<JointPtr>();
        }
        chain.push_back(pj);
        curr = pl;
        pl = curr->getParent();
        // if (pl) ROS_INFO_STREAM("Parent: "<<pl->name);
    }
    if (curr->name != from_link->name)
    {
        ROS_ERROR_STREAM("UrdfTraverser: could not find link " <<
                         from_link->name << " while traversing up the chain starting from " <<
                         to_link->name << ". Failed to find parent chain!");
        return std::vector<JointPtr>();
    }

    std::reverse(chain.begin(), chain.end());

    return chain;
}

enum uvc_frame_format CameraDriver::GetVideoMode(std::string vmode){
  if(vmode == "uncompressed") {
    return UVC_COLOR_FORMAT_UNCOMPRESSED;
  } else if (vmode == "compressed") {
    return UVC_COLOR_FORMAT_COMPRESSED;
  } else if (vmode == "yuyv") {
    return UVC_COLOR_FORMAT_YUYV;
  } else if (vmode == "uyvy") {
    return UVC_COLOR_FORMAT_UYVY;
  } else if (vmode == "rgb") {
    return UVC_COLOR_FORMAT_RGB;
  } else if (vmode == "bgr") {
    return UVC_COLOR_FORMAT_BGR;
  } else if (vmode == "mjpeg") {
    return UVC_COLOR_FORMAT_MJPEG;
  } else if (vmode == "gray8") {
    return UVC_COLOR_FORMAT_GRAY8;
  } else {
    ROS_ERROR_STREAM("Invalid Video Mode: " << vmode);
    ROS_WARN_STREAM("Continue using video mode: uncompressed");
    return UVC_COLOR_FORMAT_UNCOMPRESSED;
  }
};

    void DisableLinkPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
    {
        model_ = _parent;
        world_ = model_->GetWorld();

        // Check for link element
        if (!_sdf->HasElement("link")) {
            ROS_ERROR("No link element present. DisableLinkPlugin could not be loaded.");
            return;
        }

        link_name_ = _sdf->GetElement("link")->Get<std::string>();

        // Get pointers to joints
        link_ = model_->GetLink(link_name_);
        if (link_) {
            link_->SetEnabled(false);
            // Output some confirmation
            ROS_INFO_STREAM("DisableLinkPlugin loaded! Link: \"" << link_name_);
        }
        else
            ROS_ERROR_STREAM("Link" << link_name_ << " not found! DisableLinkPlugin could not be loaded.");
    }

	CameraDriver( int camera_index = DEFAULT_CAMERA_INDEX )
	:
		nh( "~" ),
		it( nh ),
		camera( camera_index )
	{
		nh.param<int>( "camera_index", camera_index, DEFAULT_CAMERA_INDEX );
		if ( not camera.isOpened() )
		{
			ROS_ERROR_STREAM( "Failed to open camera device!" );
			ros::shutdown();
		}

		nh.param<int>( "fps", fps, DEFAULT_FPS );
		ros::Duration period = ros::Duration( 1. / fps );

		pub_image_raw = it.advertise( "image_raw", 1 );
	
		frame = boost::make_shared< cv_bridge::CvImage >();
		frame->encoding = sensor_msgs::image_encodings::BGR8;

		timer = nh.createTimer( period, &CameraDriver::capture, this );
	}

boost::shared_ptr<pcl17::PolygonMesh> SceneCloudView::convertToMesh(float* triangles, unsigned int maxverts)
{
    if (maxverts == 0)
        return boost::shared_ptr<pcl17::PolygonMesh>();

    pcl17::PointCloud<pcl17::PointXYZ> cloud;
    cloud.width = (int)(maxverts);
    cloud.height = 1;

    cloud.points.clear();
    for (uint i = 0; i < 3 * maxverts; i += 3)
        cloud.points.push_back(pcl17::PointXYZ(triangles[i], triangles[i + 1], triangles[i + 2]));

    boost::shared_ptr<pcl17::PolygonMesh> mesh_ptr(new pcl17::PolygonMesh());

    try
    {
        pcl17::toROSMsg(cloud, mesh_ptr->cloud);
    }
    catch (std::runtime_error e)
    {
        ROS_ERROR_STREAM("Error in converting cloud to image message: "
                         << e.what());
    }

    mesh_ptr->polygons.resize(maxverts / 3);

    for (size_t i = 0; i < mesh_ptr->polygons.size(); ++i)
    {
        pcl17::Vertices v;
        v.vertices.push_back(i * 3 + 0);
        v.vertices.push_back(i * 3 + 2);
        v.vertices.push_back(i * 3 + 1);
        mesh_ptr->polygons[i] = v;
    }
    return mesh_ptr;
}

void DataCollector::onMouse( int event, int x, int y, int, void* ptr) {
  if( event != cv::EVENT_LBUTTONDOWN )
    return;

  try {
    DataCollector* that  = (DataCollector*) ptr;
    std::string image_path1 = makePngName(std::to_string(that->label), that->sub_dir);
    std::string image_path2 = makeJp2Name(std::to_string(that->label), that->sub_dir);
    if( imwrite(image_path1, that->rgb_ptr->image) && imwrite(image_path2, that->depth_ptr->image)) {
      ROS_INFO_STREAM("Write to:" << image_path1<<"\n"<<image_path2);
      that->label++;
    } else {
      ROS_ERROR_STREAM("error writing!");
    }
    
    Mat depthRead = imread(image_path2, CV_LOAD_IMAGE_ANYDEPTH);
    FeatureExtractor FE(that->rgb_ptr->image ,depthRead);
    FE.extract_feats();
    FE.visualize_feats();
  }
  catch (cv::Exception& ex) {
    fprintf(stderr, "Exception converting image to PNG format: %s\n", ex.what());
  }
}

int main(int argc, char **argv) {
  // Initialize the ROS system and become a node.
  ros::init(argc, argv, "count_and_log");
  ros::NodeHandle nh;

  // Generate log messages of varying severity regularly
  ros::Rate rate(10);
  for (int i = 1; ros::ok(); i++) {
    ROS_DEBUG_STREAM("Counted to " << i);
    if((i % 3) == 0) {
      ROS_INFO_STREAM(i << " is divisible by 3.");
    }
    if((i % 5) == 0) {
      ROS_WARN_STREAM(i << " is divisivle by 5.");
    }
    if((i % 10) == 0) {
      ROS_ERROR_STREAM(i << " is divisible by 10.");
    }
    if((i % 20) == 0) {
    ROS_FATAL_STREAM(i << " is divisible by 20.");
    }
    rate.sleep();
   }
}

  void VrpnTrackerRos::init(std::string tracker_name, ros::NodeHandle nh, bool create_mainloop_timer)
  {
    ROS_INFO_STREAM("Creating new tracker " << tracker_name);

    tracker_remote_->register_change_handler(this, &VrpnTrackerRos::handle_pose);
    tracker_remote_->register_change_handler(this, &VrpnTrackerRos::handle_twist);
    tracker_remote_->register_change_handler(this, &VrpnTrackerRos::handle_accel);
    tracker_remote_->shutup = true;

    std::string error;
    if (!ros::names::validate(tracker_name, error))
    {
      ROS_ERROR_STREAM("Invalid tracker name " << tracker_name << ", not creating topics : " << error);
      return;
    }

    this->tracker_name = tracker_name;

    output_nh_ = ros::NodeHandle(nh, tracker_name);

    std::string frame_id;
    nh.param<std::string>("frame_id", frame_id, "world");
    nh.param<bool>("use_server_time", use_server_time_, false);
    nh.param<bool>("broadcast_tf", broadcast_tf_, false);
    nh.param<bool>("process_sensor_id", process_sensor_id_, false);

    pose_msg_.header.frame_id = twist_msg_.header.frame_id = accel_msg_.header.frame_id = transform_stamped_.header.frame_id = frame_id;

    if (create_mainloop_timer)
    {
      double update_frequency;
      nh.param<double>("update_frequency", update_frequency, 100.0);
      mainloop_timer = nh.createTimer(ros::Duration(1 / update_frequency),
                                      boost::bind(&VrpnTrackerRos::mainloop, this));
    }
  }

 bool registerPlanningScene(std_srvs::Empty::Request &req,
                            std_srvs::Empty::Response &res)
 {
   register_lock_.lock();
   if(req.__connection_header->find("callerid") == req.__connection_header->end()) {
     ROS_ERROR_STREAM("Request has no callerid");
     return false;
   }
   std::string callerid = req.__connection_header->find("callerid")->second;
   if(sync_planning_scene_clients_.find(callerid) != sync_planning_scene_clients_.end()) {
     delete sync_planning_scene_clients_[callerid];
   }
   sync_planning_scene_clients_[callerid] = new actionlib::SimpleActionClient<arm_navigation_msgs::SyncPlanningSceneAction>(callerid+"/"+SYNC_PLANNING_SCENE_NAME, true);
   if(!sync_planning_scene_clients_[callerid]->waitForServer(ros::Duration(10.0))) {
     ROS_INFO_STREAM("Couldn't connect back to action server for " << callerid << ". Removing from list");
     delete sync_planning_scene_clients_[callerid];
     sync_planning_scene_clients_.erase(callerid);
     register_lock_.unlock();
     return false;
   } 
   ROS_INFO_STREAM("Successfully connected to planning scene action server for " << callerid);
   register_lock_.unlock();
   return true;
 }

void move_to_wait_position(move_group_interface::MoveGroup& move_group)
{
  //ROS_ERROR_STREAM("move_to_wait_position is not implemented yet.  Aborting."); exit(1);

  // task variables
  bool success; // saves the move result

  // set robot wait target
  /* Fill Code: [ use the 'setNamedTarget' method in the 'move_group' object] */
  move_group.setNamedTarget(cfg.WAIT_POSE_NAME);

  // move the robot
  /* Fill Code: [ use the 'move' method in the 'move_group' object and save the result in the 'success' variable] */
  success = move_group.move();
  if(success)
  {
    ROS_INFO_STREAM("Move " << cfg.WAIT_POSE_NAME<< " Succeeded");
  }
  else
  {
    ROS_ERROR_STREAM("Move " << cfg.WAIT_POSE_NAME<< " Failed");
    exit(1);
  }
}

void TinkerforgeSensors::publishTemperatureMessage(SensorDevice *sensor)
{
  if (sensor != NULL)
  {
    int16_t temperature;
    if(temperature_get_temperature((Temperature*)sensor->getDev(), &temperature) < 0) {
        ROS_ERROR_STREAM("Could not get temperature from " << sensor->getUID() << ", probably timeout");
        return;
    }
    // generate Temperature message from temperature sensor
    sensor_msgs::Temperature temp_msg;

    // message header
    temp_msg.header.seq =  sensor->getSeq();
    temp_msg.header.stamp = ros::Time::now();
    temp_msg.header.frame_id = sensor->getFrame();

    temp_msg.temperature = temperature / 100.0;
    temp_msg.variance = 0;

    // publish Temperature msg to ros
    sensor->getPub().publish(temp_msg);
  }
}

int Cp1616IOController::changePnioMode(PNIO_MODE_TYPE requested_mode)
{
  PNIO_UINT32 error_code;
  PNIO_UINT32 valid_cp_handle = cp_handle_;

  //set required mode
  error_code = PNIO_set_mode(cp_handle_, requested_mode);

  if(error_code != PNIO_OK)
  {
    ROS_ERROR_STREAM("Not able to change IO_Controller mode: Error 0x%x" << (int)error_code);
    PNIO_close(cp_handle_);     //Close PNIO_Controller
    return (int)error_code;
  }

  if(cp_handle_ == valid_cp_handle)  //check if cp_handle_ still valid
  {
    //wait for a callback_for_mode_change_indication
    while(!sem_mod_change_){
    usleep(WAIT_FOR_CALLBACKS_PERIOD);
    }

    setSemModChange(0);
  }

  //check if the current mode is correct
  if(cp_current_mode_ != requested_mode)
  {
    ROS_ERROR("Not able to set required mode: ERROR another mode recieved");
    PNIO_close(cp_handle_);
  }
  else
    ROS_INFO("Changing IO_controller mode: done");

  return (int)error_code;
}

	bool TeleOpJoypad::moveGripper(std::string joint_position_name)
	{
		brics_actuator::JointPositions pos;
		XmlRpc::XmlRpcValue position_list;
		std::string param_name = "/script_server/gripper/" + joint_position_name;

		// get gripper values
		if(!nh_->getParam(param_name, position_list))
		{
			ROS_ERROR_STREAM("Could not find parameter <<" << param_name << " on parameter server");
			return false;
		}

		ROS_ASSERT(position_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
		ROS_ASSERT(position_list.size() == 1);
		ROS_ASSERT(position_list[0].size() == 2);


		// establish messgage
		brics_actuator::JointValue gripper_left;
		gripper_left.joint_uri = "gripper_finger_joint_l";
		gripper_left.unit = "m";
		gripper_left.value = static_cast<double>(position_list[0][0]);

		brics_actuator::JointValue gripper_right;
		gripper_right.joint_uri = "gripper_finger_joint_r";
		gripper_right.unit = "m";
		gripper_right.value = static_cast<double>(position_list[0][1]);

		pos.positions.push_back(gripper_left);
		pos.positions.push_back(gripper_right);

		pub_gripper_position_.publish(pos);

		return true;
	}

  /** Main driver loop */
  bool spin()
  {
    while (nh_.ok())
      {
        getParameters();                // check reconfigurable parameters

        // get current CameraInfo data
        cam_info_ = cinfo_->getCameraInfo();
        cloud2_.header.frame_id = cloud_.header.frame_id = 
	  image_d_.header.frame_id = image_i_.header.frame_id = 
	  image_c_.header.frame_id = image_d16_.header.frame_id = 
	  cam_info_.header.frame_id = camera_name_;//config_.frame_id;

        if(!device_open_)
          {
            try
              {
		if (dev_->open(config_.auto_exposure, config_.integration_time,
			       modulation_freq_, config_.amp_threshold, ether_addr_) == 0)
                  {
                    ROS_INFO_STREAM("[" << camera_name_ << "] Connected to device with ID: "
                                    << dev_->device_id_);
                    ROS_INFO_STREAM("[" << camera_name_ << "] libmesasr version: " << dev_->lib_version_);
                    device_open_ = true; 
                  }
                else
                  {
                    ros::Duration(3.0).sleep();
                  }
              }
            catch (sr::Exception& e)
              {
                ROS_ERROR_STREAM("Exception thrown while connecting to the camera: "
                                 << e.what ());
                ros::Duration(3.0).sleep();
              }
          }
        else
          {
            try
              {
                // Read data from the Camera
                dev_->readData(cloud_,cloud2_,image_d_, image_i_, image_c_, image_d16_);
 
                cam_info_.header.stamp = image_d_.header.stamp;
                cam_info_.height = image_d_.height;
                cam_info_.width = image_d_.width;

                // Publish it via image_transport
                if (info_pub_.getNumSubscribers() > 0)
                  info_pub_.publish(cam_info_);
                if (image_pub_d_.getNumSubscribers() > 0)
                  image_pub_d_.publish(image_d_);
                if (image_pub_i_.getNumSubscribers() > 0)
                  image_pub_i_.publish(image_i_);
                if (image_pub_c_.getNumSubscribers() > 0)
                  image_pub_c_.publish(image_c_);
                if (image_pub_d16_.getNumSubscribers() > 0)
                  image_pub_d16_.publish(image_d16_);
                if (cloud_pub_.getNumSubscribers() > 0)
                  cloud_pub_.publish (cloud_);
                if (cloud_pub2_.getNumSubscribers() > 0)
                  cloud_pub2_.publish (cloud2_);
              }
            catch (sr::Exception& e) {
              ROS_WARN("Exception thrown trying to read data: %s",
                       e.what());
              dev_->close();
              device_open_ = false;
              ros::Duration(3.0).sleep();
              }
          }
        ros::spinOnce();
      }

    return true;
  }

/***
 * Starts up a SimpleGraspControlServer.
 * Launch this node with the following launch file (or include it in another launch file):
 *
 * `` \`rospack find grasp_execution\`/launch/simple_grasp_control_server.launch `` 
 *
 * Please also refer to this file (and the SimpleGraspControlServer header documentation)
 * for more details about the required parameters.
 *
 * \author Jennifer Buehler
 * \date March 2016
 */
int main(int argc, char**argv){
	ros::init(argc, argv, "simple_grasp_action");

	ros::NodeHandle priv("~");
	ros::NodeHandle pub;

	std::string JOINT_STATES_TOPIC="/joint_states";
	priv.param<std::string>("joint_states_topic", JOINT_STATES_TOPIC, JOINT_STATES_TOPIC);
	
    std::string JOINT_CONTROL_TOPIC="/joint_control";
	priv.param<std::string>("joint_control_topic", JOINT_CONTROL_TOPIC, JOINT_CONTROL_TOPIC);

	std::string GRASP_ACTION_TOPIC="/grasp_control_action";
	priv.param<std::string>("grasp_control_action_topic", GRASP_ACTION_TOPIC, GRASP_ACTION_TOPIC);
	
    std::string ROBOT_NAMESPACE;
	if (!priv.hasParam("robot_namespace"))
    {
        ROS_ERROR_STREAM(ros::this_node::getName()<<": Must have at least 'robot_namespace' defined in private node namespace");
        return 0;
    }
	priv.param<std::string>("robot_namespace", ROBOT_NAMESPACE, ROBOT_NAMESPACE);

	double CHECK_FINGER_STATE_RATE=DEFAULT_CHECK_FINGER_STATE_RATE;
	priv.param<double>("check_movement_rate", CHECK_FINGER_STATE_RATE, CHECK_FINGER_STATE_RATE);
	
	double NO_MOVE_TOLERANCE=DEFAULT_NO_MOVE_TOLERANCE;
	priv.param<double>("no_move_tolerance", NO_MOVE_TOLERANCE, NO_MOVE_TOLERANCE);
	
    int NO_MOVE_STILL_CNT=DEFAULT_NO_MOVE_STILL_CNT;
	priv.param<int>("no_move_still_cnt", NO_MOVE_STILL_CNT, NO_MOVE_STILL_CNT);

	double GOAL_TOLERANCE=DEFAULT_GOAL_TOLERANCE;
	priv.param<double>("goal_tolerance", GOAL_TOLERANCE, GOAL_TOLERANCE);

    ROS_INFO("Launching arm components name manager");
    arm_components_name_manager::ArmComponentsNameManager jointsManager(ROBOT_NAMESPACE, false);
    float maxWait=5;
    ROS_INFO("Waiting for joint info parameters to be loaded...");
    if (!jointsManager.waitToLoadParameters(1,maxWait,1))
    {
        ROS_ERROR("Joint names (ArmComponentsNameManager) could not be launched due to missing ROS parameters.");
        return 0;
    }

	grasp_execution::SimpleGraspControlServer actionServer(
        pub,
        GRASP_ACTION_TOPIC,
        JOINT_STATES_TOPIC, 
		JOINT_CONTROL_TOPIC,
        jointsManager,
        GOAL_TOLERANCE,
        NO_MOVE_TOLERANCE,
        NO_MOVE_STILL_CNT,
        CHECK_FINGER_STATE_RATE);

	actionServer.init();

    // ros::MultiThreadedSpinner spinner(4); // Use 4 threads
    // spinner.spin(); // spin() will not return until the node has been shutdown
	ros::spin();
    return 0;
}

SlamNode::SlamNode(void)
{
  ros::NodeHandle prvNh("~");
  int iVar                   = 0;
  double gridPublishInterval = 0.0;
  double loopRateVar         = 0.0;
  double truncationRadius    = 0.0;
  double cellSize            = 0.0;
  unsigned int octaveFactor  = 0;
  double xOffset = 0.0;
  double yOffset = 0.0;
  std::string topicLaser;
  prvNh.param<int>("robot_nbr", iVar, 1);
  unsigned int robotNbr = static_cast<unsigned int>(iVar);
  prvNh.param<double>("x_off_factor", _xOffFactor, 0.5);
  prvNh.param<double>("y_off_factor", _yOffFactor, 0.5);
  prvNh.param<double>("x_offset", xOffset, 0.0);
  prvNh.param<double>("y_offset", yOffset, 0.0);


  prvNh.param<int>("map_size", iVar, 10);
  octaveFactor = static_cast<unsigned int>(iVar);
  prvNh.param<double>("cellsize", cellSize, 0.025);
  prvNh.param<int>("truncation_radius", iVar, 3);
  truncationRadius = static_cast<double>(iVar);
  prvNh.param<double>("occ_grid_time_interval", gridPublishInterval, 2.0);
  prvNh.param<double>("loop_rate", loopRateVar, 40.0);
  prvNh.param<std::string>("laser_topic", topicLaser, "scan");

  _loopRate = new ros::Rate(loopRateVar);
  _gridInterval = new ros::Duration(gridPublishInterval);

  if(octaveFactor > 15)
  {
    ROS_ERROR_STREAM("Error! Unknown map size -> set to default!" << std::endl);
    octaveFactor = 10;
  }
  //instanciate representation
  _grid = new obvious::TsdGrid(cellSize, obvious::LAYOUT_32x32, static_cast<obvious::EnumTsdGridLayout>(octaveFactor));  //obvious::LAYOUT_8192x8192
  _grid->setMaxTruncation(truncationRadius * cellSize);
  unsigned int cellsPerSide = pow(2, octaveFactor);
  double sideLength = static_cast<double>(cellsPerSide) * cellSize;
  ROS_INFO_STREAM("Creating representation with " << cellsPerSide << "x" << cellsPerSide << "cells, representating " <<
                  sideLength << "x" << sideLength << "m^2" << std::endl);
  //instanciate mapping threads
  _threadMapping = new ThreadMapping(_grid);
  _threadGrid    = new ThreadGrid(_grid, &_nh, xOffset, yOffset);

  ThreadLocalize* threadLocalize = NULL;
  ros::Subscriber subs;
  std::string nameSpace = "";

  //instanciate localization threads
  if(robotNbr == 1)  //single slam
  {
    threadLocalize = new ThreadLocalize(_grid, _threadMapping, &_nh, nameSpace, xOffset, yOffset);
    subs = _nh.subscribe(topicLaser, 1, &ThreadLocalize::laserCallBack, threadLocalize);
    _subsLaser.push_back(subs);
    _localizers.push_back(threadLocalize);
    ROS_INFO_STREAM("Single SLAM started" << std::endl);
  }
  else
  {
    for(unsigned int i = 0; i < robotNbr; i++)   //multi slam
    {
      std::stringstream sstream;
      sstream << "robot";
      sstream << i << "/namespace";
      std::string dummy = sstream.str();
      prvNh.param(dummy, nameSpace, std::string("default_ns"));
      threadLocalize = new ThreadLocalize(_grid, _threadMapping, &_nh, nameSpace, xOffset, yOffset);
      subs = _nh.subscribe(nameSpace + "/" + topicLaser, 1, &ThreadLocalize::laserCallBack, threadLocalize);
      _subsLaser.push_back(subs);
      _localizers.push_back(threadLocalize);
      ROS_INFO_STREAM("started for thread for " << nameSpace << std::endl);
    }
    ROS_INFO_STREAM("Multi SLAM started!");
  }
}