void MentalPerspectiveTransformer::callback(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud_in) {
    static tf::TransformListener listener;
    const std::string source_frame = "robot_head";
    const std::string target_frame = "head_origin1";

    // first get cloud in the source frame (it is usually in camera frame)
    pcl::PointCloud<pcl::PointXYZRGB> cloud_in_source_frame;
    pcl_ros::transformPointCloud(source_frame, *cloud_in, cloud_in_source_frame, listener);

    static tf::TransformBroadcaster br;
    try {
        // get transform from source to target frame
        tf::StampedTransform transform_source_target;
        listener.waitForTransform(target_frame, source_frame, pcl_conversions::fromPCL(cloud_in_source_frame.header.stamp), ros::Duration(1.0));
        listener.lookupTransform (target_frame, source_frame, ros::Time(0), transform_source_target);

        ROS_INFO_STREAM("Origin: " << transform_source_target.getOrigin().getX() << " "
                                   << transform_source_target.getOrigin().getY() << " "
                                   << transform_source_target.getOrigin().getZ());
        ROS_INFO_STREAM("Angle: "  << transform_source_target.getRotation().getAngle());
        ROS_INFO_STREAM("Axis: "   << transform_source_target.getRotation().getAxis().getX() << " "
                                   << transform_source_target.getRotation().getAxis().getY() << " "
                                   << transform_source_target.getRotation().getAxis().getZ());

        tf::Quaternion q_origin       = tf::Quaternion::getIdentity();
        tf::Quaternion q_dest         = transform_source_target.getRotation();
        tf::Vector3    v_dest         = transform_source_target.getOrigin();

        //ROS_INFO_STREAM("q_dest: " << q_dest.getX() << " " << q_dest.getY() << " " << q_dest.getZ() << " " << q_dest.getW());

        /*tf::Vector3 v_r               = v_dest/2.0;
        ROS_INFO_STREAM("v_r: " << v_r.getX() << " " << v_r.getY() << " " << v_r.getZ());
        tf::Vector3 c                 = tf::Vector3(-1.0*v_r[1], v_r[0], 0);
        tf::Vector3 p_circle          = v_r + c;
        ROS_INFO_STREAM("p_circle " << p_circle.getX() << " " << p_circle.getY() << " " << p_circle.getZ());
        double radius                 = v_r.length();
        ROS_INFO_STREAM("radius " << radius);
        tf::Vector3 n = p_circle.cross(v_r).normalize();
        if(transform.getRotation().getAngle()>M_PI) {
            n = -1.0*n;
        }

        ROS_INFO_STREAM("n " << n.getX() << " " << n.getY() << " " << n.getZ());
        tf::Vector3 u                 = v_dest.normalized();
        ROS_INFO_STREAM("u " << u.getX() << " " << u.getY() << " " << u.getZ());*/

        // compute circle equation
        double r1x = (pow(v_dest.getX(), 2.0) + pow(v_dest.getY(), 2.0) + pow(v_dest.getZ(), 2.0)) / (2.0*(v_dest.getX()+pow(v_dest.getZ(), 2.0)/v_dest.getX()));
        double r1y = 0.0;
        double r1z = r1x * v_dest.getZ()/v_dest.getX();

        tf::Vector3 r1(r1x, r1y, r1z);
        tf::Vector3 r2(v_dest.getX() - r1x, v_dest.getY() - r1y, v_dest.getZ() - r1z);

        double radius = r1.length();

        tf::Vector3 n = v_dest.cross(2*r1).normalize();
        tf::Vector3 u = r1.normalized();
        tf::Vector3 v_r = r1;

        ROS_INFO_STREAM("u " << u.getX() << " " << u.getY() << " " << u.getZ());
        ROS_INFO_STREAM("n " << n.getX() << " " << n.getY() << " " << n.getZ());
        ROS_INFO_STREAM("r1 " << r1x << " " << r1y << " " << r1z);
        ROS_INFO_STREAM("r2 " << r2.getX() << " " << r2.getY() << " " << r2.getZ());
        ROS_INFO_STREAM("radius " << radius);

        // get theta for head coordinates
        perspective_taking_python::CircleEquation ce;
        ce.request.radius = radius;
        tf::vector3TFToMsg(v_dest, ce.request.h);
        tf::vector3TFToMsg(n, ce.request.n);
        tf::vector3TFToMsg(u, ce.request.u);
        tf::vector3TFToMsg(v_r, ce.request.c);
        if(equationSolverClient_.call(ce)) {
            ROS_INFO_STREAM("response: " << ce.response.theta);
        } else {
            ROS_ERROR("Did not get response from equationSolverClient!");
            return;
        }

        double delta = 0.01; // approximately 10 steps at 1.3 meters distance to human (straight ahead)
        double epsilon = M_PI * delta / radius;
        double resp_theta = ce.response.theta;
        ROS_INFO_STREAM("epsilon: " << epsilon);

        for(double theta = -1.0, step=0; theta<=resp_theta; theta+=epsilon, step+=1) {
            double b = 1.0/(resp_theta+1.0);
            if(transform_source_target.getRotation().getAngle()>M_PI) {
                b = -1.0*b;
            }
            double theta_for_q = b * theta + b;
            tf::Quaternion q_intermediate = q_origin.slerp(q_dest, theta_for_q);
            //tf::Quaternion q_intermediate = q_origin.slerp(q_dest, 1.0-theta);
            //tf::Vector3 v_intermediate    = v_origin.lerp(v_dest, ration);
            tf::Vector3 v_intermediate    = radius*cos(M_PI*theta)*u + radius*sin(M_PI*theta)*n.cross(u)+v_r;

            ROS_INFO_STREAM("theta_for_q: " << theta_for_q);
            ROS_INFO_STREAM("theta: " << theta << ": " << v_intermediate.getX() << " " << v_intermediate.getY() << " " << v_intermediate.getZ());

            tf::Transform intermediate_transform;
            intermediate_transform.setOrigin(v_intermediate);
            intermediate_transform.setRotation(q_intermediate);
            tf::Transform intermediate_transform_inv = intermediate_transform.inverse();

            std::stringstream ss; ss << std::fixed << setprecision(2) << step;
            br.sendTransform(tf::StampedTransform(intermediate_transform_inv, ros::Time::now(), source_frame, "rot"+ss.str()));

            ROS_INFO_STREAM("step: " << step);
        }

        pcl::PointCloud<pcl::PointXYZRGB> output;
        pcl_ros::transformPointCloud(target_frame, cloud_in_source_frame, output, listener);
        transformedCloudPublisher_.publish(output);
    }
    catch (tf::TransformException ex){
        ROS_ERROR("%s",ex.what());
        ros::Duration(1.0).sleep();
    }
}
Beispiel #2
0
bool WifiStumbler::stumble()
{
  int pid;
  pid = getpid();

  struct iwreq w_request;
  w_request.u.data.pointer = (caddr_t)&pid;
  w_request.u.data.length = 0;

  if (iw_set_ext(wlan_sock_, wlan_if_.c_str(), SIOCSIWSCAN, &w_request) < 0)
  {
    ROS_ERROR("Couldn't start stumbler.");
    return false;
  }

  timeval time;
  time.tv_sec = 0;
  time.tv_usec = 200000;

  uint8_t *p_buff = NULL;
  int buff_size = IW_SCAN_MAX_DATA;

  bool is_end = false;
  while(!is_end)
  {
    fd_set fds;
    int ret;
    FD_ZERO(&fds);
    ret = select(0, &fds, NULL, NULL, &time);
    if (ret == 0)
    {
      uint8_t *p_buff2;
      while (!is_end)
      {
        p_buff2 = (uint8_t *)realloc(p_buff, buff_size);
        p_buff = p_buff2;
        w_request.u.data.pointer = p_buff;
        w_request.u.data.flags = 0;
        w_request.u.data.length = buff_size;
        if (iw_get_ext(wlan_sock_, wlan_if_.c_str(), SIOCGIWSCAN, &w_request) < 0)
        {
          if (errno == E2BIG && range_.we_version_compiled > 16)
          {
            if (w_request.u.data.length > buff_size)
              buff_size = w_request.u.data.length;
            else
              buff_size *= 2;
            continue;
          }
          else if (errno == EAGAIN)
          {
            time.tv_sec = 0;
            time.tv_usec = 200000;
            break;
          }
        }
        else
          is_end = true;
      }
    }
  }

  // Put wifi data into ROS message
  wifi_tools::AccessPoint ap;
  iw_event event;
  stream_descr stream;

  wifi_stumble_.data.clear();
  wifi_stumble_.header.stamp = ros::Time::now();

  iw_init_event_stream(&stream, (char *)p_buff, w_request.u.data.length);
  is_end = false;
  int value = 0;
  while(!is_end)
  {
    value = iw_extract_event_stream(&stream, &event, range_.we_version_compiled);
    if(!(value>0))
    {
      is_end = true;
    }
    else
    {
      if(event.cmd == IWEVQUAL)
      {
        // quality part of statistics
        //ROS_INFO("command=IWEVQUAL");
        if (event.u.qual.level != 0 || (event.u.qual.updated & (IW_QUAL_DBM | IW_QUAL_RCPI)))
        {
          ap.noise = event.u.qual.noise;
          ap.ss    = event.u.qual.level;
        }
      }
      else if(event.cmd == SIOCGIWAP)
      {
        // get access point MAC addresses
        //ROS_INFO("command=SIOCGIWAP");
        char mac_buff[1024];
        iw_ether_ntop((const struct ether_addr *)&event.u.ap_addr.sa_data,mac_buff);
        ap.mac_address = std::string(mac_buff);
        ROS_INFO("mac_addr=%s",mac_buff);
      }
      else if(event.cmd == SIOCGIWESSID)
      {
        // get ESSID
        //ROS_INFO("command=SIOCGIWESSID");
        wifi_stumble_.data.push_back(ap);
      }
      else if(event.cmd == SIOCGIWENCODE)
      {
        // get encoding token & mode
        //ROS_INFO("command=SIOCGIWENCODE");
      }
      else if(event.cmd == SIOCGIWFREQ)
      {
        // get channel/frequency (Hz)
        //ROS_INFO("command=SIOCGIWFREQ");
      }
      else if(event.cmd == SIOCGIWRATE)
      {
        // get default bit rate (bps)
        //ROS_INFO("command=SIOCGIWRATE");
      }
      else if(event.cmd == SIOCGIWMODE)
      {
        // get operation mode
        //ROS_INFO("command=SIOCGIWMODE");

      }
      else if(event.cmd == IWEVCUSTOM)
      {
        // Driver specific ascii string
        //ROS_INFO("command=IWEVCUSTOM");
      }
      else if(event.cmd == IWEVGENIE)
      {
        // Generic IE (WPA, RSN, WMM, ..)
        //ROS_INFO("command=IWEVGENIE");
      }
      else
      {
        // another command
        //ROS_INFO("another command");
      }
    }
  }

  checkDuplication();

  return true;
}
Beispiel #3
0
    // Function needs working on
    void mapUpdate(double x_sens_dist, double y_sens_dist, int sensor)
    {

        //x_sens_dist and y_sens_dist are distance from robot to detected object.
        // Always start at center of grid: add/subtract to center_x to all distances
        // according to sign conventions.
        // First, convert sensor readings from robot frame to map frame then and then convert

        int x_sens_cell = floor(x_sens_dist/resolution);
        int y_sens_cell = floor(y_sens_dist/resolution);
        int weight_cell = 5;
        double corner_x, corner_y;
        int corner_x_cell, corner_y_cell;
        tf::StampedTransform transform;
        switch(sensor)
        {
            case 1:
            try
            {
                listener.waitForTransform("/sensor1", "/map", ros::Time(0), ros::Duration(1));
                listener.lookupTransform("/map", "/sensor1", ros::Time(0), transform);
                corner_x = transform.getOrigin().getX();
                corner_y = transform.getOrigin().getY();
                corner_x_cell = floor(corner_x/resolution);
                corner_y_cell = floor(corner_y/resolution);
                ros::Time::init();
            }
                catch(tf::TransformException ex)
                {
                    ROS_ERROR("%s",ex.what());
                }
            break;


            case 2:
            try
            {
                listener.waitForTransform("/sensor2", "/map", ros::Time(0), ros::Duration(1));
                listener.lookupTransform("/map", "/sensor2", ros::Time(0), transform);
                corner_x = transform.getOrigin().getX();
                corner_y = transform.getOrigin().getY();
                corner_x_cell = floor(corner_x/resolution);
                corner_y_cell = floor(corner_y/resolution);
            }
                catch(tf::TransformException ex)
            {
                    ROS_ERROR("%s",ex.what());
            }
            break;

            case 3:
            try
            {
                listener.waitForTransform("/sensor3", "/map", ros::Time(0), ros::Duration(1));
                listener.lookupTransform("/map", "/sensor3", ros::Time(0), transform);
                corner_x = transform.getOrigin().getX();
                corner_y = transform.getOrigin().getY();
                corner_x_cell = floor(corner_x/resolution);
                corner_y_cell = floor(corner_y/resolution);
            }
                catch(tf::TransformException ex)
            {
                    ROS_ERROR("%s",ex.what());
            }
            break;

            case 4:
            try
            {
                listener.waitForTransform("/sensor4", "/map", ros::Time(0), ros::Duration(1));
                listener.lookupTransform("/map", "/sensor4", ros::Time(0), transform);
                corner_x = transform.getOrigin().getX();
                corner_y = transform.getOrigin().getY();
                corner_x_cell = floor(corner_x/resolution);
                corner_y_cell = floor(corner_y/resolution);
            }
                catch(tf::TransformException ex)
            {
                    ROS_ERROR("%s",ex.what());
            }

        }


        //map_vector[x_sens_cell+width_map*y_sens_cell]=100;
        int x1, x2, y1, y2;
        if (corner_x_cell > x_sens_cell) {
            x1 = x_sens_cell;
            x2 = corner_x_cell;
        } else {
            x1 = corner_x_cell;
            x2 = x_sens_cell;
        }
        if (corner_y_cell > y_sens_cell) {
            y1 = y_sens_cell;
            y2 = corner_y_cell;
        } else {
            y1 = corner_y_cell;
            y2 = y_sens_cell;
        }
        for(int i = x2 ; i >= x1; i--)
        {
            for(int j = y2 ; j >= y1; j--)
            {
                if((i==x_sens_cell) && (j==y_sens_cell))
                {
                    if(map_vector[i+width_map*j] == -1)
                        map_vector[i+width_map*j] = 5;
                    else
                        map_vector[i+width_map*j] = map_vector[i+width_map*j] + weight_cell;
                }
                else
                {
                    if(map_vector[i+width_map*j]==-1)
                    {
                        map_vector[i+width_map*j] = 0;
                    }
                }
            }
        }
    }
void
FootstepNavigation::executeFootsteps()
{
  if (ivPlanner.getPathSize() <= 1)
    return;

  // lock this thread
  ivExecutingFootsteps = true;

  ROS_INFO("Start walking towards the goal.");

  humanoid_nav_msgs::StepTarget step;
  humanoid_nav_msgs::StepTargetService step_srv;

  tf::Transform from;
  std::string support_foot_id;

  // calculate and perform relative footsteps until goal is reached
  state_iter_t to_planned = ivPlanner.getPathBegin();
  if (to_planned == ivPlanner.getPathEnd())
  {
    ROS_ERROR("No plan available. Return.");
    return;
  }

  const State* from_planned = to_planned.base();
  to_planned++;
  while (to_planned != ivPlanner.getPathEnd())
  {
    try
    {
      boost::this_thread::interruption_point();
    }
    catch (const boost::thread_interrupted&)
    {
      // leave this thread
      return;
    }

    if (from_planned->getLeg() == RIGHT)
      support_foot_id = ivIdFootRight;
    else // support_foot = LLEG
      support_foot_id = ivIdFootLeft;

    // try to get real placement of the support foot
    if (getFootTransform(support_foot_id, ivIdMapFrame, ros::Time::now(),
                         ros::Duration(0.5), &from))
    {
      // calculate relative step and check if it can be performed
      if (getFootstep(from, *from_planned, *to_planned, &step))
      {
        step_srv.request.step = step;
        ivFootstepSrv.call(step_srv);
      }
      // ..if it cannot be performed initialize replanning
      else
      {
        ROS_INFO("Footstep cannot be performed. Replanning necessary.");

        replan();
        // leave the thread
        return;
      }
    }
    else
    {
      // if the support foot could not be received wait and try again
      ros::Duration(0.5).sleep();
      continue;
    }

    from_planned = to_planned.base();
    to_planned++;
  }
  ROS_INFO("Succeeded walking to the goal.\n");

  // free the lock
  ivExecutingFootsteps = false;
}
bool GazeboRosControllerManager::LoadControllerManagerFromURDF()
{
  std::string urdf_string = GetURDF(this->robotParam);

  // initialize TiXmlDocument doc with a string
  TiXmlDocument doc;
  if (!doc.Parse(urdf_string.c_str()) && doc.Error())
  {
    ROS_ERROR("Could not load the gazebo controller manager plugin's"
              " configuration file: %s\n", urdf_string.c_str());
    return false;
  }
  else
  {
    // debug
    // doc.Print();
    // std::cout << *(doc.RootElement()) << std::endl;

    // Pulls out the list of actuators used in the robot configuration.
    struct GetActuators : public TiXmlVisitor
    {
      std::set<std::string> actuators;
      virtual bool VisitEnter(const TiXmlElement &elt, const TiXmlAttribute *)
      {
        if (elt.ValueStr() == std::string("actuator") && elt.Attribute("name"))
          actuators.insert(elt.Attribute("name"));
        else if (elt.ValueStr() ==
          std::string("rightActuator") && elt.Attribute("name"))
          actuators.insert(elt.Attribute("name"));
        else if (elt.ValueStr() ==
          std::string("leftActuator") && elt.Attribute("name"))
          actuators.insert(elt.Attribute("name"));
        return true;
      }
    } get_actuators;
    doc.RootElement()->Accept(&get_actuators);

    // Places the found actuators into the hardware interface.
    std::set<std::string>::iterator it;
    for (it = get_actuators.actuators.begin();
         it != get_actuators.actuators.end(); ++it)
    {
      // std::cout << " adding actuator " << (*it) << std::endl;
      pr2_hardware_interface::Actuator* pr2_actuator =
        new pr2_hardware_interface::Actuator(*it);
      pr2_actuator->state_.is_enabled_ = true;
      this->hardware_interface_.addActuator(pr2_actuator);
    }

    // Setup mechanism control node
    this->controller_manager_->initXml(doc.RootElement());

    if (this->controller_manager_->state_ == NULL)
    {
      ROS_ERROR("Mechanism unable to parse robot_description URDF"
                " to fill out robot state in controller_manager.");
      return false;
    }

    // set fake calibration states for simulation
    for (unsigned int i = 0;
      i < this->controller_manager_->state_->joint_states_.size(); ++i)
      this->controller_manager_->state_->joint_states_[i].calibrated_ =
        calibration_status_;

    return true;
  }
}
  boost::shared_ptr<kinematics::KinematicsBase> allocKinematicsSolver(const robot_model::JointModelGroup *jmg)
  {
    boost::shared_ptr<kinematics::KinematicsBase> result;
    if (!jmg)
    {
      ROS_ERROR("Specified group is NULL. Cannot allocate kinematics solver.");
      return result;
    }

    ROS_DEBUG("Received request to allocate kinematics solver for group '%s'", jmg->getName().c_str());

    if (kinematics_loader_ && jmg)
    {
      std::map<std::string, std::vector<std::string> >::const_iterator it = possible_kinematics_solvers_.find(jmg->getName());
      if (it != possible_kinematics_solvers_.end())
      {
        // just to be sure, do not call the same pluginlib instance allocation function in parallel
        boost::mutex::scoped_lock slock(lock_);

        for (std::size_t i = 0 ; !result && i < it->second.size() ; ++i)
        {
          try
          {
            result.reset(kinematics_loader_->createUnmanagedInstance(it->second[i]));
            if (result)
            {
              const std::vector<const robot_model::LinkModel*> &links = jmg->getLinkModels();
              if (!links.empty())
              {
                const std::string &base = links.front()->getParentJointModel()->getParentLinkModel() ?
                  links.front()->getParentJointModel()->getParentLinkModel()->getName() : jmg->getParentModel().getModelFrame();

                // choose the tip of the IK solver
                const std::vector<std::string> tips = chooseTipFrames(jmg);

                // choose search resolution
                double search_res = search_res_.find(jmg->getName())->second[i]; // we know this exists, by construction

                if (!result->initialize(robot_description_, jmg->getName(),
                                        (base.empty() || base[0] != '/') ? base : base.substr(1) , tips, search_res))
                {
                  ROS_ERROR("Kinematics solver of type '%s' could not be initialized for group '%s'", it->second[i].c_str(), jmg->getName().c_str());
                  result.reset();
                }
                else
                {
                  result->setDefaultTimeout(jmg->getDefaultIKTimeout());
                  ROS_DEBUG("Successfully allocated and initialized a kinematics solver of type '%s' with search resolution %lf for group '%s' at address %p",
                            it->second[i].c_str(), search_res, jmg->getName().c_str(), result.get());
                }
              }
              else
                ROS_ERROR("No links specified for group '%s'", jmg->getName().c_str());
            }
          }
          catch (pluginlib::PluginlibException& e)
          {
            ROS_ERROR("The kinematics plugin (%s) failed to load. Error: %s", it->first.c_str(), e.what());
          }
        }
      }
      else
        ROS_DEBUG("No kinematics solver available for this group");
    }

    if (!result)
    {
      ROS_DEBUG("No usable kinematics solver was found for this group.");
      ROS_DEBUG("Did you load kinematics.yaml into your node's namespace?");
    }
    return result;
  }
Beispiel #7
0
int main(int argc, char **argv)
{
	int rate= 50;
	float inv_rate=1/rate;

	ros::init(argc, argv, "ARdrone_PID_to_Point");
	ros::NodeHandle n;
	ros::Rate loop_rate(rate);
	
	tf::TransformListener tf_listener;
	tf::TransformBroadcaster br;

	tf::StampedTransform desired_pos;
	tf::StampedTransform ardrone;
	tf::StampedTransform trackee;
	tf::StampedTransform desired_in_ardrone_coords;

	ros::Publisher pub_twist;
	geometry_msgs::Twist twist_msg;
	tf::Quaternion ardrone_yawed;
	
	memset(controls, 0, sizeof(double)*4);
	memset(old_data, 0, sizeof(double)*5);
	memset(new_data, 0, sizeof(double)*5);
	memset(integration, 0, sizeof(double)*5);
	memset(derivative, 0, sizeof(double)*5);
	memset(pid, 0, sizeof(double)*4);
	
	//PID params
	min_control[yaw] =-1.0;
	min_control[roll] =-1.0;
	min_control[pitch]=-1.0;
	min_control[thrust]=-1.0;

	max_control[yaw]=1.0;
	max_control[roll]=1.0;
	max_control[pitch]=1.0;
	max_control[thrust]=1.0;

	min_pid =-5.0;
	max_pid =5.0;

	while (ros::ok())
	{
		  try {
		//Get desired position transform
		tf_listener.waitForTransform("/optitrak", "/desired_position", ros::Time(0), ros::Duration(inv_rate));
		tf_listener.lookupTransform("/optitrak", "/desired_position",  ros::Time(0), desired_pos); 
		// Get the quad rotor transform
		tf_listener.waitForTransform("/optitrak", "/ardrone", ros::Time(0), ros::Duration(inv_rate));
		tf_listener.lookupTransform("/optitrak", "/ardrone",  ros::Time(0), ardrone);

		} catch (...) {
		  	ROS_ERROR("Failed on initial transform: Check VRPN server");}
	
		  double y1, p1, r1;
		  btMatrix3x3(ardrone.getRotation()).getRPY(r1, p1, y1);
		  
		  ardrone_yawed.setRPY(0.0,0.0,y1);
		  
		  /* //Dep code 
		  btQuaternion ardrone_yawed(y1, 0.0, 0.0);
		  */
		  
		  ardrone.setRotation(ardrone_yawed);

		  //set up twist publisher
		  pub_twist = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1); /* Message queue length is just 1 */
		
		  // Register the ardrone without roll and pitch with the transform system
		  br.sendTransform( tf::StampedTransform(ardrone, ros::Time::now(), "/optitrak", "ardrone_wo_rp") );
		  
		  try {
		  // Get the vector between quad without roll and pitch and the desired point
		  tf_listener.waitForTransform("/ardrone_wo_rp", "/desired", ros::Time(0), ros::Duration(inv_rate));
		  tf_listener.lookupTransform("/ardrone_wo_rp", "/desired", ros::Time(0), desired_in_ardrone_coords);
		  }
		  catch (...) {
		  	ROS_ERROR("Failed on w/o roll and pitch transform");}
		  	
		  // Extract the yaw, x, y, z components
		  btMatrix3x3(desired_in_ardrone_coords.getRotation()).getRPY(r1, p1, y1);
		  new_data[yaw]=y1;
		  new_data[pitch] = desired_in_ardrone_coords.getOrigin().getX();
		  new_data[roll] = desired_in_ardrone_coords.getOrigin().getY();
		  new_data[thrust] = desired_in_ardrone_coords.getOrigin().getZ();
 		  new_data[msg_time] = (double)ros::Time::now().toSec();
		  ROS_DEBUG("Error: [x: %f y:  %f z: %f]", new_data[pitch], new_data[roll], new_data[thrust]);

		  // Integrate/Derivate the data
		  double deltaT = (new_data[msg_time] - old_data[msg_time]);
		  integration[yaw] += new_data[yaw] * deltaT;
		  integration[pitch] += new_data[pitch] * deltaT;
		  integration[roll] += new_data[roll] * deltaT;
		  integration[thrust] += new_data[thrust] * deltaT;

		  ROS_DEBUG("Integration: [deltaT: %f x: %f y: %f z: %f]", deltaT, integration[pitch], integration[roll], integration[thrust]);

		  derivative[yaw] = (new_data[yaw] - old_data[yaw])/deltaT;
		  derivative[pitch] = (new_data[pitch] - old_data[pitch])/deltaT;
		  derivative[roll] = (new_data[roll] - old_data[roll])/deltaT;
		  derivative[thrust] = (new_data[thrust] - old_data[thrust])/deltaT;

		  ROS_DEBUG("Derivative: [deltaT: %f x: %f y: %f z: %f]", deltaT, derivative[pitch], derivative[roll], derivative[thrust]);

		  // Calculate the PID values
		  pid[yaw] = K_p[yaw] * new_data[yaw] + K_d[yaw] * derivative[yaw] + K_i[yaw] * integration[yaw];
		  pid[pitch] = K_p[pitch] * new_data[pitch] + K_d[pitch] * derivative[pitch] + K_i[pitch] * integration[pitch];
		  pid[roll] = K_p[roll] * new_data[roll] + K_d[roll] * derivative[roll] + K_i[roll] * integration[roll];
		  pid[thrust] = K_p[thrust] * new_data[thrust] + K_d[thrust] * derivative[thrust] + K_i[thrust] * integration[thrust];

		  ROS_DEBUG("PID: [x: %f y:  %f z: %f]", pid[pitch], pid[roll], pid[thrust]);

		  memcpy(old_data, new_data, sizeof(double)*5);
			
		  pid[yaw]=0.0; //YAW IS DISABLED!
		  controls[yaw] =    map(pid[yaw], 	 min_pid, max_pid, min_control[yaw], max_control[yaw]);
		  controls[pitch] =   map(pid[pitch], min_pid, max_pid, min_control[roll], max_control[roll]);
		  controls[roll] =  map(pid[roll], min_pid, max_pid, min_control[pitch], max_control[pitch]);
		  controls[thrust] = map(pid[thrust], min_pid, max_pid, min_control[thrust], max_control[thrust]);
		  ROS_DEBUG("Controls: [yaw: %f roll: %f pitch: %f thrust: %f]", controls[yaw], controls[roll], controls[pitch], controls[thrust]);

	      //change the ref frame to inverted x-y-z coords. by modifying the directional control
		  twist_msg.linear.x=-controls[roll]; 
		  twist_msg.linear.y=-controls[pitch];	
		  twist_msg.linear.z=-controls[thrust];
		  twist_msg.angular.z=controls[yaw];
          pub_twist.publish(twist_msg);
		  
			ros::spinOnce();
			loop_rate.sleep();
	
}//while ros ok      
	ROS_ERROR("ROS::ok failed- Node Closing");
		  
}//main 
Beispiel #8
0
  void ARSinglePublisher::getTransformationCallback (const sensor_msgs::ImageConstPtr & image_msg)
  {
    ARUint8 *dataPtr;
    ARMarkerInfo *marker_info;
    int marker_num;
    int i, k;

    /* Get the image from ROSTOPIC
     * NOTE: the dataPtr format is BGR because the ARToolKit library was
     * build with V4L, dataPtr format change according to the 
     * ARToolKit configure option (see config.h).*/
    try
    {
      capture_ = bridge_.imgMsgToCv (image_msg, "bgr8");
    }
    catch (sensor_msgs::CvBridgeException & e)
    {
      ROS_ERROR ("Could not convert from '%s' to 'bgr8'.", image_msg->encoding.c_str ());
    }
    //cvConvertImage(capture_,capture_,CV_CVTIMG_FLIP); //flip image
    dataPtr = (ARUint8 *) capture_->imageData;

    // detect the markers in the video frame 
    if (arDetectMarker (dataPtr, threshold_, &marker_info, &marker_num) < 0)
    {
      ROS_FATAL ("arDetectMarker failed");
      ROS_BREAK ();             // FIXME: I don't think this should be fatal... -Bill
    }

    // check for known patterns
    k = -1;
    for (i = 0; i < marker_num; i++)
    {
      if (marker_info[i].id == patt_id_)
      {
        ROS_DEBUG ("Found pattern: %d ", patt_id_);

        // make sure you have the best pattern (highest confidence factor)
        if (k == -1)
          k = i;
        else if (marker_info[k].cf < marker_info[i].cf)
          k = i;
      }
    }

    if (k != -1)
    {
      // **** get the transformation between the marker and the real camera
      double arQuat[4], arPos[3];

      if (!useHistory_ || contF == 0)
        arGetTransMat (&marker_info[k], marker_center_, markerWidth_, marker_trans_);
      else
        arGetTransMatCont (&marker_info[k], marker_trans_, marker_center_, markerWidth_, marker_trans_);

      contF = 1;

      //arUtilMatInv (marker_trans_, cam_trans);
      arUtilMat2QuatPos (marker_trans_, arQuat, arPos);

      // **** convert to ROS frame

      double quat[4], pos[3];
    
      pos[0] = arPos[0] * AR_TO_ROS;
      pos[1] = arPos[1] * AR_TO_ROS;
      pos[2] = arPos[2] * AR_TO_ROS;

      quat[0] = -arQuat[0];
      quat[1] = -arQuat[1];
      quat[2] = -arQuat[2];
      quat[3] = arQuat[3];

      ROS_DEBUG (" QUAT: Pos x: %3.5f  y: %3.5f  z: %3.5f", pos[0], pos[1], pos[2]);
      ROS_DEBUG ("     Quat qx: %3.5f qy: %3.5f qz: %3.5f qw: %3.5f", quat[0], quat[1], quat[2], quat[3]);

      // **** publish the marker

		  ar_pose_marker_.header.frame_id = image_msg->header.frame_id;
		  ar_pose_marker_.header.stamp    = image_msg->header.stamp;
		  ar_pose_marker_.id              = marker_info->id;

		  ar_pose_marker_.pose.pose.position.x = pos[0];
		  ar_pose_marker_.pose.pose.position.y = pos[1];
		  ar_pose_marker_.pose.pose.position.z = pos[2];

		  ar_pose_marker_.pose.pose.orientation.x = quat[0];
		  ar_pose_marker_.pose.pose.orientation.y = quat[1];
		  ar_pose_marker_.pose.pose.orientation.z = quat[2];
		  ar_pose_marker_.pose.pose.orientation.w = quat[3];
		
		  ar_pose_marker_.confidence = marker_info->cf;

		  arMarkerPub_.publish(ar_pose_marker_);
		  ROS_DEBUG ("Published ar_single marker");
		
      // **** publish transform between camera and marker

      btQuaternion rotation (quat[0], quat[1], quat[2], quat[3]);
      btVector3 origin(pos[0], pos[1], pos[2]);
      btTransform t(rotation, origin);

      if(publishTf_)
      {
        if(reverse_transform_)
        {
          tf::StampedTransform markerToCam (t.inverse(), image_msg->header.stamp, markerFrame_.c_str(), image_msg->header.frame_id);
          broadcaster_.sendTransform(markerToCam);
        } else {
          tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame_.c_str());
          broadcaster_.sendTransform(camToMarker);
        }
      }

      // **** publish visual marker

      if(publishVisualMarkers_)
      {
        btVector3 markerOrigin(0, 0, 0.25 * markerWidth_ * AR_TO_ROS);
        btTransform m(btQuaternion::getIdentity(), markerOrigin);
        btTransform markerPose = t * m; // marker pose in the camera frame
      
        tf::poseTFToMsg(markerPose, rvizMarker_.pose);

			  rvizMarker_.header.frame_id = image_msg->header.frame_id;
			  rvizMarker_.header.stamp = image_msg->header.stamp;
			  rvizMarker_.id = 1;

			  rvizMarker_.scale.x = 1.0 * markerWidth_ * AR_TO_ROS;
			  rvizMarker_.scale.y = 1.0 * markerWidth_ * AR_TO_ROS;
			  rvizMarker_.scale.z = 0.5 * markerWidth_ * AR_TO_ROS;
			  rvizMarker_.ns = "basic_shapes";
			  rvizMarker_.type = visualization_msgs::Marker::CUBE;
			  rvizMarker_.action = visualization_msgs::Marker::ADD;
			  rvizMarker_.color.r = 0.0f;
			  rvizMarker_.color.g = 1.0f;
			  rvizMarker_.color.b = 0.0f;
			  rvizMarker_.color.a = 1.0;
			  rvizMarker_.lifetime = ros::Duration();
			
			  rvizMarkerPub_.publish(rvizMarker_);
			  ROS_DEBUG ("Published visual marker");
      }
    }
    else
    {
      contF = 0;
      ROS_DEBUG ("Failed to locate marker");
    }
  }
void costmap_cb(const nav_msgs::GridCells::ConstPtr& msg) {
    ODOM_FRAME = msg->header.frame_id;
    
    geometry_msgs::PoseArray ff_msg;
    ff_msg.header.stamp = msg->header.stamp;
    ff_msg.header.frame_id = ODOM_FRAME;
    
    last_fv = msg->header.stamp;
    force_vector[0] = force_vector[1] = 0;
    std::vector<geometry_msgs::Point> points = msg->cells;
    
    std::vector<geometry_msgs::Pose> poses;
    
    int count = 0;
    float total_weight = 0.0;
    
    for (std::vector<std::string>::size_type i = 0; i < points.size(); i++) {
        geometry_msgs::PointStamped point, point_trans;
        point.header.stamp = msg->header.stamp;
        point.header.frame_id = ODOM_FRAME;
        point.point.x = points[i].x;
        point.point.y = points[i].y;
        tf_listener->waitForTransform(BASE_FRAME, ODOM_FRAME, point.header.stamp, ros::Duration(0.1));
        try {
            tf_listener->transformPoint(BASE_FRAME, point, point_trans);
        }
        catch (tf::ExtrapolationException e) {
            ROS_ERROR("Unable to get tf transform: %s", e.what());
            return;
        }
        
        float x = point_trans.point.x;
        float y = point_trans.point.y;
        float yaw = atan(y / x);
        if (x > 0) yaw = modulus(yaw + PI, 2*PI);
        
        std::vector<float> point_vector = point_to_vector(x, y);
        
        if (point_vector.size() > 0) {
            count++;
            //float weight = 0.75*(MAX_RANGE - BASE_RADIUS - CLEARING_DIST)/(sqrt(pow(x, 2) + pow(y, 2)) - BASE_RADIUS - CLEARING_DIST);
            //if ( weight < 0 )             
            //    std::cout << "evil" << std::endl;
            //length += weight;
            //float weight = (PI - abs(yaw)) / PI;
            force_vector[0] += point_vector[0];
            force_vector[1] += point_vector[1];

            geometry_msgs::PoseStamped pose, pose_trans;
            pose.header.stamp = msg->header.stamp;
            pose.header.frame_id = BASE_FRAME;
            pose.pose.position.x = x;
            pose.pose.position.y = y;
            pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);

            tf_listener->waitForTransform(ODOM_FRAME, BASE_FRAME, pose.header.stamp, ros::Duration(0.1));
            try {
                tf_listener->transformPose(ODOM_FRAME, pose, pose_trans);
            }
            catch (tf::ExtrapolationException e) {
                ROS_ERROR("Unable to get tf transform: %s", e.what());
                return;
            }
            
            poses.push_back(pose_trans.pose);
        }
    }
    
    //force_vector[0] /= length + 1;
    //force_vector[1] /= length + 1;
    
    std::cout << force_vector[0] << "," << force_vector[1] << std::endl;
    
    // publish resulting force vector as Pose
    geometry_msgs::PoseStamped force, force_trans;
    force.header.stamp = msg->header.stamp;
    force.header.frame_id = BASE_FRAME;
    float force_yaw = atan(force_vector[1] / force_vector[0]);
    if (force_vector[0] > 0) force_yaw = modulus(force_yaw + PI, 2*PI);
    force.pose.orientation = tf::createQuaternionMsgFromYaw(force_yaw);    
    tf_listener->waitForTransform(BASE_FRAME, ODOM_FRAME, msg->header.stamp, ros::Duration(0.1));
    try {
        tf_listener->transformPose(ODOM_FRAME, force, force_trans);
    }
    catch (tf::ExtrapolationException e) {
        ROS_ERROR("Unable to get tf transform: %s", e.what());
        return;
    }
    force_obst_pub.publish(force_trans);    
    
    // publish force field as PoseArray
    ff_msg.poses = poses;  
    force_field_pub.publish(ff_msg);
}
int main(int argc, char **argv)
{
  // Init the ROS node
  ros::init (argc, argv, "move_right_arm_joint_goal_test");

  // Precondition: Valid clock
  ros::NodeHandle nh;
  if (!ros::Time::waitForValid(ros::WallDuration(5.0))) // NOTE: Important when using simulated clock
  {
    ROS_FATAL("Timed-out waiting for valid time.");
    return EXIT_FAILURE;
  }

  // Action client for sending motion planing requests
  actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_right_arm_torso", true);

  // Wait for the action client to be connected to the server
  ROS_INFO("Connecting to server...");
  if (!move_arm.waitForServer(ros::Duration(5.0)))
  {
    ROS_ERROR("Timed-out waiting for the move_arm action server.");
    return EXIT_FAILURE;
  }
  ROS_INFO("Connected to server.");

  // Prepare motion plan request with joint-space goal
  arm_navigation_msgs::MoveArmGoal goal;
  std::vector<std::string> names(9);
  names[0] = "torso_1_joint";
  names[1] = "torso_2_joint";
  names[2] = "arm_right_1_joint";
  names[3] = "arm_right_2_joint";
  names[4] = "arm_right_3_joint";
  names[5] = "arm_right_4_joint";
  names[6] = "arm_right_5_joint";
  names[7] = "arm_right_6_joint";
  names[8] = "arm_right_7_joint";

  goal.motion_plan_request.group_name = "right_arm_torso";
  goal.motion_plan_request.num_planning_attempts = 1;
  goal.motion_plan_request.allowed_planning_time = ros::Duration(5.0);

  goal.motion_plan_request.planner_id= std::string("");
  goal.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
  goal.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());

  for (unsigned int i = 0 ; i < goal.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
  {
    goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
    goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
    goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.05;
    goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.05;
  }

  goal.motion_plan_request.goal_constraints.joint_constraints[0].position =  0.0;
  goal.motion_plan_request.goal_constraints.joint_constraints[1].position =  0.0;
  goal.motion_plan_request.goal_constraints.joint_constraints[2].position =  1.2;
  goal.motion_plan_request.goal_constraints.joint_constraints[3].position =  0.15;
  goal.motion_plan_request.goal_constraints.joint_constraints[4].position = -1.5708;
  goal.motion_plan_request.goal_constraints.joint_constraints[5].position =  1.3;
  goal.motion_plan_request.goal_constraints.joint_constraints[6].position =  0.0;
  goal.motion_plan_request.goal_constraints.joint_constraints[7].position =  0.0;
  goal.motion_plan_request.goal_constraints.joint_constraints[8].position =  0.0;

  // Send motion plan request
  if (nh.ok())
  {
    bool finished_within_time = false;
    move_arm.sendGoal(goal);
    finished_within_time = move_arm.waitForResult(ros::Duration(15.0));
    if (!finished_within_time)
    {
      move_arm.cancelGoal();
      ROS_INFO("Timed out achieving joint-space goal.");
    }
    else
    {
      actionlib::SimpleClientGoalState state = move_arm.getState();
      bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
      if(success)
        ROS_INFO("Action finished: %s",state.toString().c_str());
      else
        ROS_INFO("Action failed: %s",state.toString().c_str());
    }
  }
  ros::shutdown();
}
void PclExtractor::cloudCallback(const Cloud2Msg::ConstPtr& cloud2Msg_input)
{

  boost::mutex::scoped_lock(mutex_);
  sensor_msgs::PointCloud2 output;
  sensor_msgs::PointCloud2 convex_hull;
  sensor_msgs::PointCloud2 object_msg;
  sensor_msgs::PointCloud2 nonObject_msg;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg (*cloud2Msg_input, *cloud);


  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);

  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
  
  // *** Normal estimation
  // Create the normal estimation class and pass the input dataset to it
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
  ne.setInputCloud(cloud);
  // Creating the kdTree object for the search method of the extraction
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
  ne.setSearchMethod(tree);

  // output dataset
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);

  // use all neighbors in a sphere of radius 3cm
  ne.setRadiusSearch(0.3);

  // compute the features
  ne.compute(*cloud_normals);
  // *** End of normal estimation
  // *** Plane Estimation From Normals Start
  // Create the segmentation object
  pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
  // Optional
  seg.setOptimizeCoefficients(true);
  // Mandatory
//  seg.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
  seg.setModelType(pcl::SACMODEL_PLANE);
//  seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
  //y가 z
//  const Eigen::Vector3f z_axis(0,-1.0,0);
//  seg.setAxis(z_axis);
//  seg.setEpsAngle(seg_setEpsAngle_);
  seg.setMethodType(pcl::SAC_RANSAC);
  seg.setMaxIterations (seg_setMaxIterations_);
  seg.setDistanceThreshold(seg_setDistanceThreshold_);
  seg.setNormalDistanceWeight(seg_setNormalDistanceWeight_);
//  seg.setProbability(seg_probability_);

  seg.setInputCloud((*cloud).makeShared());
  seg.setInputNormals(cloud_normals);
  seg.segment(*inliers, *coefficients);

  std::cerr <<"input: "<<cloud->width*cloud->height<<"Model inliers: " << inliers->indices.size () << std::endl;
  if (inliers->indices.size () == 0)
  {
    ROS_ERROR ("Could not estimate a planar model for the given dataset.");
  }
  // *** End of Plane Estimation
  // *** Plane Estimation Start
  // Create the segmentation object
/*  pcl::SACSegmentation<pcl::PointXYZ> seg;
  // Optional
  //seg.setOptimizeCoefficients(true);
  // Mandatory
  seg.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
//  seg.setModelType(pcl::SACMODEL_PLANE);
//  seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
  //y가 z
  const Eigen::Vector3f z_axis(0,-1.0,0);
  seg.setAxis(z_axis);
  seg.setEpsAngle(seg_setEpsAngle_);
  seg.setMethodType(pcl::SAC_RANSAC);
  seg.setMaxIterations (seg_setMaxIterations_);
  seg.setDistanceThreshold(seg_setDistanceThreshold_);

  seg.setInputCloud((*cloud).makeShared());
  seg.segment(*inliers, *coefficients);

  std::cerr <<"input: "<<cloud->width*cloud->height<<"Model inliers: " << inliers->indices.size () << std::endl;
  if (inliers->indices.size () == 0)
  {
    ROS_ERROR ("Could not estimate a planar model for the given dataset.");
  }
  // *** End of Plane Estimation
*/
  pcl::ExtractIndices<pcl::PointXYZ> extract;
  // Extrat the inliers
  extract.setInputCloud(cloud);
  extract.setIndices(inliers);

  pcl::PointCloud<pcl::PointXYZ>::Ptr ground_hull(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);

  if ((int)inliers->indices.size() > minPoints_)
  {
    extract.setNegative(false);
    extract.filter(*cloud_p);

    pcl::toROSMsg(*cloud_p, output);
    std::cerr << "PointCloud representing the planar component: " 
      << cloud_p->width * cloud_p->height << " data points." << std::endl;

    // Step 3c. Project the ground inliers
    pcl::ProjectInliers<pcl::PointXYZ> proj;
    proj.setModelType(pcl::SACMODEL_PLANE);
    proj.setInputCloud(cloud_p);
    proj.setModelCoefficients(coefficients);
    proj.filter(*cloud_projected);
    // Step 3d. Create a Convex Hull representation of the projected inliers
    pcl::ConvexHull<pcl::PointXYZ> chull;
    //chull.setInputCloud(cloud_p);
    chull.setInputCloud(cloud_projected);
    chull.setDimension(chull_setDimension_);//2D
    chull.reconstruct(*ground_hull);
    pcl::toROSMsg(*ground_hull, convex_hull);
    //pcl::toROSMsg(*ground_hull, convex_hull);
    ROS_INFO ("Convex hull has: %d data points.", (int) ground_hull->points.size ());

    // Publish the data
    plane_pub_.publish (output);
    hull_pub_.publish(convex_hull);

  }
  // Create the filtering object
  extract.setNegative(true);
  extract.filter(*cloud_f);
  ROS_INFO ("Cloud_f has: %d data points.", (int) cloud_f->points.size ());
  // cloud.swap(cloud_f);

  pcl::PointCloud<pcl::PointXYZ>::Ptr object(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr nonObject(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointIndices::Ptr object_indices(new pcl::PointIndices);

  // cloud statictical filter
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudStatisticalFiltered (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
  sor.setInputCloud(cloud_f);
  sor.setMeanK(sor_setMeanK_);
  sor.setStddevMulThresh(sor_setStddevMulThresh_);
  sor.filter(*cloudStatisticalFiltered);

  pcl::ExtractIndices<pcl::PointXYZ> exObjectIndices;
  //exObjectIndices.setInputCloud(cloud_f);
  exObjectIndices.setInputCloud(cloudStatisticalFiltered);
  exObjectIndices.setIndices(object_indices);
  exObjectIndices.filter(*object);
  exObjectIndices.setNegative(true);
  exObjectIndices.filter(*nonObject);

  ROS_INFO ("Object has: %d data points.", (int) object->points.size ());
  pcl::toROSMsg(*object, object_msg);
  object_pub_.publish(object_msg);
  //pcl::toROSMsg(*nonObject, nonObject_msg);
  //pcl::toROSMsg(*cloud_f, nonObject_msg);
  pcl::toROSMsg(*cloudStatisticalFiltered, nonObject_msg);
  nonObject_pub_.publish(nonObject_msg);
}
Beispiel #12
0
void Sub8StereoHandler::generate_point_cloud(
                                             const sensor_msgs::ImageConstPtr &msg_l,
                                             const sensor_msgs::ImageConstPtr &msg_r,
                                             pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pcl_ptr, ros::Publisher &pc_pub) {
    // NOTE: Calls to this function will block until the Synchronizer is able to
    // secure
    //       the corresponding messages.
    float px, py, pz;
    cv_bridge::CvImagePtr cv_ptr_l;
    cv_bridge::CvImagePtr cv_ptr_r;
    cv_bridge::CvImagePtr cv_ptr_bgr;
    
    try {
        cv_ptr_l = cv_bridge::toCvCopy(msg_l, sensor_msgs::image_encodings::MONO8);
        cv_ptr_r = cv_bridge::toCvCopy(msg_r, sensor_msgs::image_encodings::MONO8);
        
        // Used to reproject color onto pointcloud (We'll just use the left image)
        cv_ptr_bgr = cv_bridge::toCvCopy(msg_l, sensor_msgs::image_encodings::BGR8);
    } catch (cv_bridge::Exception &e) {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    
    cv::gpu::Stream pc_stream = cv::gpu::Stream();
    this->d_left.upload(cv_ptr_l->image);
    this->d_right.upload(cv_ptr_r->image);
    
    switch (this->matching_method) {
        case 0:
            if (this->d_left.channels() > 1 || this->d_right.channels() > 1) {
                // TODO: Add handle for these cases
                ROS_WARN("Block-matcher does not support color images");
            }
            this->bm(this->d_left, this->d_right, this->d_disp, pc_stream);
            break; // BM
        case 1:
            this->bp(this->d_left, this->d_right, this->d_disp, pc_stream);
            break; // BP
        case 2:
            this->csbp(this->d_left, this->d_right, this->d_disp, pc_stream);
            break; // CSBP
    }
    
    // cv::Mat h_disp, h_3d_img, h_point;
    
    cv::gpu::reprojectImageTo3D(this->d_disp, this->gpu_pdisp, this->Q_, 4,
                                pc_stream);
    this->gpu_pdisp.download(this->pdisp);
    
    for (int i = 0; i < this->pdisp.rows; i++) {
        // For future reference OpenCV is ROW-MAJOR
        for (int j = 0; j < this->pdisp.cols; j++) {
            cv::Point3f ptxyz = this->pdisp.at<cv::Point3f>(i, j);
            
            if (!isinf(ptxyz.x) && !isinf(ptxyz.y) && !isinf(ptxyz.z)) {
                // Reproject color points onto distances
                cv::Vec3b color_info = cv_ptr_bgr->image.at<cv::Vec3b>(i, j);
                pcl::PointXYZRGB pcl_pt;
                
                pcl_pt.x = ptxyz.x;
                pcl_pt.y = ptxyz.y;
                pcl_pt.z = ptxyz.z;
                
                uint32_t rgb = (static_cast<uint32_t>(color_info[2]) << 16 |
                                static_cast<uint32_t>(color_info[1]) << 8 |
                                static_cast<uint32_t>(color_info[0]));
                pcl_pt.rgb = *reinterpret_cast<float *>(&rgb);
                pcl_ptr->points.push_back(pcl_pt);
            }
        }
    }
    
    pcl_ptr->width = (int)pcl_ptr->points.size();
    pcl_ptr->height = 1;
    
    pcl::toROSMsg(*pcl_ptr, this->stereo_pc);
    this->stereo_pc.header.frame_id = "/stereo_front";
    pcl_ptr->clear();
    pc_pub.publish(this->stereo_pc);
}
Beispiel #13
0
  void ARMultiPublisher::getTransformationCallback (const sensor_msgs::ImageConstPtr & image_msg)
  {
    ARUint8 *dataPtr;
    ARMarkerInfo *marker_info;
    int marker_num;
    int i, k, j;

    /* Get the image from ROSTOPIC
     * NOTE: the dataPtr format is BGR because the ARToolKit library was
     * build with V4L, dataPtr format change according to the 
     * ARToolKit configure option (see config.h).*/
#if ROS_VERSION_MINIMUM(1, 9, 0)
    try
    {
      capture_ = cv_bridge::toCvCopy (image_msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what()); 
    }
    dataPtr = (ARUint8 *) ((IplImage) capture_->image).imageData;
#else
    try
    {
      capture_ = bridge_.imgMsgToCv (image_msg, "bgr8");
    }
    catch (sensor_msgs::CvBridgeException & e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what()); 
    }
    dataPtr = (ARUint8 *) capture_->imageData;
#endif

    // detect the markers in the video frame
    if (arDetectMarker (dataPtr, threshold_, &marker_info, &marker_num) < 0)
    {
      argCleanup ();
      ROS_BREAK ();
    }

    arPoseMarkers_.markers.clear ();
    // check for known patterns
    for (i = 0; i < objectnum; i++)
    {
      k = -1;
      for (j = 0; j < marker_num; j++)
      {
        if (object[i].id == marker_info[j].id)
        {
          if (k == -1)
            k = j;
          else                  // make sure you have the best pattern (highest confidence factor)
          if (marker_info[k].cf < marker_info[j].cf)
            k = j;
        }
      }
      if (k == -1)
      {
        object[i].visible = 0;
        continue;
      }

      // calculate the transform for each marker
      if (object[i].visible == 0)
      {
        arGetTransMat (&marker_info[k], object[i].marker_center, object[i].marker_width, object[i].trans);
      }
      else
      {
        arGetTransMatCont (&marker_info[k], object[i].trans,
                           object[i].marker_center, object[i].marker_width, object[i].trans);
      }
      object[i].visible = 1;

      double arQuat[4], arPos[3];

      //arUtilMatInv (object[i].trans, cam_trans);
      arUtilMat2QuatPos (object[i].trans, arQuat, arPos);

      // **** convert to ROS frame

      double quat[4], pos[3];

      pos[0] = arPos[0] * AR_TO_ROS;
      pos[1] = arPos[1] * AR_TO_ROS;
      pos[2] = arPos[2] * AR_TO_ROS;

      if (isRightCamera_) {
        pos[2] += 0; // -0.001704;
        pos[0] += 0; // +0.0899971;
        pos[1] += 0; // -0.00012;
      }

      quat[0] = -arQuat[0];
      quat[1] = -arQuat[1];
      quat[2] = -arQuat[2];
      quat[3] = arQuat[3];

      ROS_DEBUG (" Object num %i ------------------------------------------------", i);
      ROS_DEBUG (" QUAT: Pos x: %3.5f  y: %3.5f  z: %3.5f", pos[0], pos[1], pos[2]);
      ROS_DEBUG ("     Quat qx: %3.5f qy: %3.5f qz: %3.5f qw: %3.5f", quat[0], quat[1], quat[2], quat[3]);

      // **** publish the marker

      ar_pose::ARMarker ar_pose_marker;
      ar_pose_marker.header.frame_id = image_msg->header.frame_id;
      ar_pose_marker.header.stamp = image_msg->header.stamp;
      ar_pose_marker.id = object[i].id;

      ar_pose_marker.pose.pose.position.x = pos[0];
      ar_pose_marker.pose.pose.position.y = pos[1];
      ar_pose_marker.pose.pose.position.z = pos[2];

      ar_pose_marker.pose.pose.orientation.x = quat[0];
      ar_pose_marker.pose.pose.orientation.y = quat[1];
      ar_pose_marker.pose.pose.orientation.z = quat[2];
      ar_pose_marker.pose.pose.orientation.w = quat[3];

      ar_pose_marker.confidence = round(marker_info[k].cf * 100);
      arPoseMarkers_.markers.push_back (ar_pose_marker);

      // **** publish transform between camera and marker

#if ROS_VERSION_MINIMUM(1, 9, 0)
      btQuaternion rotation (quat[0], quat[1], quat[2], quat[3]);
      btVector3 origin (pos[0], pos[1], pos[2]);
      btTransform t (rotation, origin);
#else
// DEPRECATED: Fuerte support ends when Hydro is released
      btQuaternion rotation (quat[0], quat[1], quat[2], quat[3]);
      btVector3 origin (pos[0], pos[1], pos[2]);
      btTransform t (rotation, origin);
#endif

      if (publishTf_)
      {
        std::string name = object[i].name;
        if (isRightCamera_) {
          name += "_r";
        }
        tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, name);
        broadcaster_.sendTransform(camToMarker);
      }

      // **** publish visual marker

      if (publishVisualMarkers_)
      {
#if ROS_VERSION_MINIMUM(1, 9, 0)
        btVector3 markerOrigin (0, 0, 0.25 * object[i].marker_width * AR_TO_ROS);
        btTransform m (btQuaternion::getIdentity (), markerOrigin);
        btTransform markerPose = t * m; // marker pose in the camera frame 
#else
// DEPRECATED: Fuerte support ends when Hydro is released
        btVector3 markerOrigin (0, 0, 0.25 * object[i].marker_width * AR_TO_ROS);
        btTransform m (btQuaternion::getIdentity (), markerOrigin);
        btTransform markerPose = t * m; // marker pose in the camera frame
#endif

        tf::poseTFToMsg (markerPose, rvizMarker_.pose);

        rvizMarker_.header.frame_id = image_msg->header.frame_id;
        rvizMarker_.header.stamp = image_msg->header.stamp;
        rvizMarker_.id = object[i].id;

        rvizMarker_.scale.x = 1.0 * object[i].marker_width * AR_TO_ROS;
        rvizMarker_.scale.y = 1.0 * object[i].marker_width * AR_TO_ROS;
        rvizMarker_.scale.z = 0.5 * object[i].marker_width * AR_TO_ROS;
        rvizMarker_.ns = "basic_shapes";
        rvizMarker_.type = visualization_msgs::Marker::CUBE;
        rvizMarker_.action = visualization_msgs::Marker::ADD;
        switch (i)
        {
          case 0:
            rvizMarker_.color.r = 0.0f;
            rvizMarker_.color.g = 0.0f;
            rvizMarker_.color.b = 1.0f;
            rvizMarker_.color.a = 1.0;
            break;
          case 1:
            rvizMarker_.color.r = 1.0f;
            rvizMarker_.color.g = 0.0f;
            rvizMarker_.color.b = 0.0f;
            rvizMarker_.color.a = 1.0;
            break;
          default:
            rvizMarker_.color.r = 0.0f;
            rvizMarker_.color.g = 1.0f;
            rvizMarker_.color.b = 0.0f;
            rvizMarker_.color.a = 1.0;
        }
        rvizMarker_.lifetime = ros::Duration (1.0);

        rvizMarkerPub_.publish(rvizMarker_);
        ROS_DEBUG ("Published visual marker");
      }
    }
    arMarkerPub_.publish(arPoseMarkers_);
    ROS_DEBUG ("Published ar_multi markers");
  }
  void JointStateController::addExtraJoints(const ros::NodeHandle& nh, sensor_msgs::JointState& msg)
  {

    // Preconditions
    XmlRpc::XmlRpcValue list;
    if (!nh.getParam("extra_joints", list))
    {
      ROS_DEBUG("No extra joints specification found.");
      return;
    }

    if (list.getType() != XmlRpc::XmlRpcValue::TypeArray)
    {
      ROS_ERROR("Extra joints specification is not an array. Ignoring.");
      return;
    }

    for(std::size_t i = 0; i < list.size(); ++i)
    {
      if (list[i].getType() != XmlRpc::XmlRpcValue::TypeStruct)
      {
        ROS_ERROR_STREAM("Extra joint specification is not a struct, but rather '" << list[i].getType() <<
                         "'. Ignoring.");
        continue;
      }

      if (!list[i].hasMember("name"))
      {
        ROS_ERROR_STREAM("Extra joint does not specify name. Ignoring.");
        continue;
      }

      const std::string name = list[i]["name"];
      if (std::find(msg.name.begin(), msg.name.end(), name) != msg.name.end())
      {
        ROS_WARN_STREAM("Joint state interface already contains specified extra joint '" << name << "'.");
        continue;
      }

      const bool has_pos = list[i].hasMember("position");
      const bool has_vel = list[i].hasMember("velocity");
      const bool has_eff = list[i].hasMember("effort");

      const XmlRpc::XmlRpcValue::Type typeDouble = XmlRpc::XmlRpcValue::TypeDouble;
      if (has_pos && list[i]["position"].getType() != typeDouble)
      {
        ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default position. Ignoring.");
        continue;
      }
      if (has_vel && list[i]["velocity"].getType() != typeDouble)
      {
        ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default velocity. Ignoring.");
        continue;
      }
      if (has_eff && list[i]["effort"].getType() != typeDouble)
      {
        ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default effort. Ignoring.");
        continue;
      }

      // State of extra joint
      const double pos = has_pos ? static_cast<double>(list[i]["position"]) : 0.0;
      const double vel = has_vel ? static_cast<double>(list[i]["velocity"]) : 0.0;
      const double eff = has_eff ? static_cast<double>(list[i]["effort"])   : 0.0;

      // Add extra joints to message
      msg.name.push_back(name);
      msg.position.push_back(pos);
      msg.velocity.push_back(vel);
      msg.effort.push_back(eff);
    }
  }
	// Constructor
	NodeClass(std::string name):
		as_(n_, name, boost::bind(&NodeClass::executeCB, this, _1)),
		action_name_(name)
	{
		n_ = ros::NodeHandle("~");
	
		isInitialized_ = false;
		isError_ = false;
		ActualPos_=0.0;
		ActualVel_=0.0;

		CamAxis_ = new ElmoCtrl();
		CamAxisParams_ = new ElmoCtrlParams();

		// implementation of topics to publish
		topicPub_JointState_ = n_.advertise<sensor_msgs::JointState>("/joint_states", 1);
		topicPub_ControllerState_ = n_.advertise<pr2_controllers_msgs::JointTrajectoryControllerState>("state", 1);
		topicPub_Diagnostic_ = n_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);


		// implementation of topics to subscribe
		
		// implementation of service servers
		srvServer_Init_ = n_.advertiseService("init", &NodeClass::srvCallback_Init, this);
		srvServer_Stop_ = n_.advertiseService("stop", &NodeClass::srvCallback_Stop, this);
		srvServer_Recover_ = n_.advertiseService("recover", &NodeClass::srvCallback_Recover, this);
		srvServer_SetOperationMode_ = n_.advertiseService("set_operation_mode", &NodeClass::srvCallback_SetOperationMode, this);
		srvServer_SetDefaultVel_ = n_.advertiseService("set_default_vel", &NodeClass::srvCallback_SetDefaultVel, this);
		
		// implementation of service clients
		//--

		// read parameters from parameter server
		if(!n_.hasParam("EnoderIncrementsPerRevMot")) ROS_WARN("cob_head_axis: couldn't find parameter EnoderIncrementsPerRevMot, check if ALL parameters have been set correctly");

		n_.param<std::string>("CanDevice", CanDevice_, "PCAN");
		n_.param<int>("CanBaudrate", CanBaudrate_, 500);
		n_.param<int>("HomingDir", HomingDir_, 1);
		n_.param<int>("HomingDigIn", HomingDigIn_, 11);
		n_.param<int>("ModId",ModID_, 17);
		n_.param<std::string>("JointName",JointName_, "head_axis_joint");
		n_.param<std::string>("CanIniFile",CanIniFile_, "/");
		n_.param<std::string>("operation_mode",operationMode_, "position");
		n_.param<int>("MotorDirection",MotorDirection_, 1);
		n_.param<double>("GearRatio",GearRatio_, 62.5);
		n_.param<int>("EnoderIncrementsPerRevMot",EnoderIncrementsPerRevMot_, 4096);
		
		ROS_INFO("CanDevice=%s, CanBaudrate=%d, ModID=%d, HomingDigIn=%d",CanDevice_.c_str(),CanBaudrate_,ModID_,HomingDigIn_);
		
		
		// load parameter server string for robot/model
		std::string param_name = "/robot_description";
		std::string full_param_name;
		std::string xml_string;
		n_.searchParam(param_name,full_param_name);
		n_.getParam(full_param_name.c_str(),xml_string);
		ROS_INFO("full_param_name=%s",full_param_name.c_str());
		if (xml_string.size()==0)
		{
			ROS_ERROR("Unable to load robot model from param server robot_description\n");
			exit(2);
		}
		ROS_DEBUG("%s content\n%s", full_param_name.c_str(), xml_string.c_str());
		
		// extract limits and velocitys from urdf model
		urdf::Model model;
		if (!model.initString(xml_string))
		{
			ROS_ERROR("Failed to parse urdf file");
			exit(2);
		}
		ROS_INFO("Successfully parsed urdf file");

		// get LowerLimits out of urdf model
		LowerLimit_ = model.getJoint(JointName_.c_str())->limits->lower;
			//std::cout << "LowerLimits[" << JointNames[i].c_str() << "] = " << LowerLimits[i] << std::endl;
		CamAxisParams_->SetLowerLimit(LowerLimit_);

		// get UpperLimits out of urdf model
		UpperLimit_ = model.getJoint(JointName_.c_str())->limits->upper;
			//std::cout << "LowerLimits[" << JointNames[i].c_str() << "] = " << LowerLimits[i] << std::endl;
		CamAxisParams_->SetUpperLimit(UpperLimit_);

		// get Offset out of urdf model
		Offset_ = model.getJoint(JointName_.c_str())->calibration->rising.get()[0];
			//std::cout << "Offset[" << JointNames[i].c_str() << "] = " << Offsets[i] << std::endl;
		CamAxisParams_->SetAngleOffset(Offset_);
		
		// get velocitiy out of urdf model
		MaxVel_ = model.getJoint(JointName_.c_str())->limits->velocity;
		ROS_INFO("Successfully read limits from urdf");


		//initializing and homing of camera_axis		
		CamAxisParams_->SetCanIniFile(CanIniFile_);
		CamAxisParams_->SetHomingDir(HomingDir_);
		CamAxisParams_->SetHomingDigIn(HomingDigIn_);
		CamAxisParams_->SetMaxVel(MaxVel_);
		CamAxisParams_->SetGearRatio(GearRatio_);
		CamAxisParams_->SetMotorDirection(MotorDirection_);
		CamAxisParams_->SetEncoderIncrements(EnoderIncrementsPerRevMot_);
		
		
		

		CamAxisParams_->Init(CanDevice_, CanBaudrate_, ModID_);
		

	}
Beispiel #16
0
int RosAriaNode::Setup()
{
  // Note, various objects are allocated here which are never deleted (freed), since Setup() is only supposed to be
  // called once per instance, and these objects need to persist until the process terminates.

  robot = new ArRobot();

  ArArgumentBuilder *args = new ArArgumentBuilder(); //  never freed
  ArArgumentParser *argparser = new ArArgumentParser(args); // Warning never freed
  argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args.  Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)

  // Now add any parameters given via ros params (see RosAriaNode constructor):

  // if serial port parameter contains a ':' character, then interpret it as hostname:tcpport
  // for wireless serial connection. Otherwise, interpret it as a serial port name.
  size_t colon_pos = serial_port.find(":");
  if (colon_pos != std::string::npos)
  {
    args->add("-remoteHost"); // pass robot's hostname/IP address to Aria
    args->add(serial_port.substr(0, colon_pos).c_str());
    args->add("-remoteRobotTcpPort"); // pass robot's TCP port to Aria
    args->add(serial_port.substr(colon_pos+1).c_str());
  }
  else
  {
    args->add("-robotPort"); // pass robot's serial port to Aria
    args->add(serial_port.c_str());
  }

  // if a baud rate was specified in baud parameter
  if(serial_baud != 0)
  {
    args->add("-robotBaud");
    char tmp[100];
    snprintf(tmp, 100, "%d", serial_baud);
    args->add(tmp);
  }
  
  if( debug_aria )
  {
    // turn on all ARIA debugging
    args->add("-robotLogPacketsReceived"); // log received packets
    args->add("-robotLogPacketsSent"); // log sent packets
    args->add("-robotLogVelocitiesReceived"); // log received velocities
    args->add("-robotLogMovementSent");
    args->add("-robotLogMovementReceived");
    ArLog::init(ArLog::File, ArLog::Verbose, aria_log_filename.c_str(), true);
  }

  // Connect to the robot
  conn = new ArRobotConnector(argparser, robot); // warning never freed
  if (!conn->connectRobot()) {
    ROS_ERROR("RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device.)");
    return 1;
  }

  // causes ARIA to load various robot-specific hardware parameters from the robot parameter file in /usr/local/Aria/params
  if(!Aria::parseArgs())
  {
    ROS_ERROR("RosAria: ARIA error parsing ARIA startup parameters!");
    return 1;
  }

  // Start dynamic_reconfigure server
  dynamic_reconfigure_server = new dynamic_reconfigure::Server<rosaria::RosAriaConfig>;

  robot->lock();

  // Setup Parameter Minimums
  rosaria::RosAriaConfig dynConf_min;

  //arbitrary non-zero values so dynamic reconfigure isn't STUPID
  dynConf_min.trans_vel_max = 0.1; 
  dynConf_min.rot_vel_max = 0.1; 
  dynConf_min.trans_accel = 0.1;
  dynConf_min.trans_decel = 0.1;
  dynConf_min.rot_accel = 0.1;
  dynConf_min.rot_decel = 0.1; 
  
  // I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
  dynConf_min.TicksMM     = 10;
  dynConf_min.DriftFactor = -200;
  dynConf_min.RevCount    = -32760;
  
  dynamic_reconfigure_server->setConfigMin(dynConf_min);
  
  rosaria::RosAriaConfig dynConf_max;
  dynConf_max.trans_vel_max = robot->getAbsoluteMaxTransVel() / 1000.0; 
  dynConf_max.rot_vel_max = robot->getAbsoluteMaxRotVel() *M_PI/180.0; 
  dynConf_max.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000.0;
  dynConf_max.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000.0;
  dynConf_max.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180.0;
  dynConf_max.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180.0;
  
  // I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
  dynConf_max.TicksMM     = 200;
  dynConf_max.DriftFactor = 200;
  dynConf_max.RevCount    = 32760;
  
  dynamic_reconfigure_server->setConfigMax(dynConf_max);


  dynConf_default.trans_vel_max = robot->getTransVelMax() / 1000.0; 
  dynConf_default.rot_vel_max = robot->getRotVelMax() *M_PI/180.0; 
  dynConf_default.trans_accel = robot->getTransAccel() / 1000.0;
  dynConf_default.trans_decel = robot->getTransDecel() / 1000.0;
  dynConf_default.rot_accel   = robot->getRotAccel() * M_PI/180.0;
  dynConf_default.rot_decel   = robot->getRotDecel() * M_PI/180.0;

/*  ROS_ERROR("ON ROBOT NOW\n\
Trans vel max: %f\n\
Rot vel max: %f\n\
\n\
trans accel: %f\n\
trans decel: %f\n\
rot accel: %f\n\
rot decel: %f", robot->getTransVelMax(), robot->getRotVelMax(), robot->getTransAccel(), robot->getTransDecel(), robot->getRotAccel(), robot->getRotDecel());

  ROS_ERROR("IN DEFAULT CONFIG\n\
Trans vel max: %f\n\
Rot vel max: %f\n\
\n\
trans accel: %f\n\
trans decel: %f\n\
rot accel: %f\n\
rot decel: %f\n", dynConf_default.trans_vel_max,  dynConf_default.rot_vel_max, dynConf_default.trans_accel, dynConf_default.trans_decel, dynConf_default.rot_accel, dynConf_default.rot_decel);*/

  TicksMM = robot->getOrigRobotConfig()->getTicksMM();
  DriftFactor = robot->getOrigRobotConfig()->getDriftFactor();
  RevCount = robot->getOrigRobotConfig()->getRevCount();

  dynConf_default.TicksMM     = TicksMM;
  dynConf_default.DriftFactor = DriftFactor;
  dynConf_default.RevCount    = RevCount;
  
  dynamic_reconfigure_server->setConfigDefault(dynConf_default);

  for(int i = 0; i < 16; i++)
  {
    sonar_tf_array[i].header.frame_id = frame_id_base_link;
    std::stringstream _frame_id;
    _frame_id << "sonar" << i;
    sonar_tf_array[i].child_frame_id = _frame_id.str();
    ArSensorReading* _reading = NULL;
    _reading = robot->getSonarReading(i);
    sonar_tf_array[i].transform.translation.x = _reading->getSensorX() / 1000.0;
    sonar_tf_array[i].transform.translation.y = _reading->getSensorY() / 1000.0;
    sonar_tf_array[i].transform.translation.z = 0.19;
    sonar_tf_array[i].transform.rotation = tf::createQuaternionMsgFromYaw(_reading->getSensorTh() * M_PI / 180.0);
  }

  for (int i=0;i<16;i++) {
      sensor_msgs::Range r;
      ranges.data.push_back(r);
  }

  int i=0,j=0;
  if (sonars__crossed_the_streams) {
    i=8;
    j=8;
  }
  for(; i<16; i++) {
    //populate the RangeArray msg
    std::stringstream _frame_id;
    _frame_id << "sonar" << i;
    ranges.data[i].header.frame_id = _frame_id.str();
    ranges.data[i].radiation_type = 0;
    ranges.data[i].field_of_view = 0.2618f; 
    ranges.data[i].min_range = 0.03f;
    ranges.data[i].max_range = 5.0f;
  }

  // Enable the motors
  robot->enableMotors();

  robot->disableSonar();

  // Initialize bumpers with robot number of bumpers
  bumpers.front_bumpers.resize(robot->getNumFrontBumpers());
  bumpers.rear_bumpers.resize(robot->getNumRearBumpers());

  robot->unlock();

  pose_pub = n.advertise<nav_msgs::Odometry>("pose",1000);
  bumpers_pub = n.advertise<rosaria::BumperState>("bumper_state",1000);

  voltage_pub = n.advertise<std_msgs::Float64>("battery_voltage", 1000);
  
  combined_range_pub = n.advertise<rosaria::RangeArray>("ranges", 1000,
    boost::bind(&RosAriaNode::sonarConnectCb,this),
    boost::bind(&RosAriaNode::sonarDisconnectCb, this));

  for(int i =0; i < 16; i++) {
    std::stringstream topic_name;
    topic_name << "range" << i;
    range_pub[i] = n.advertise<sensor_msgs::Range>(topic_name.str().c_str(), 1000,
      boost::bind(&RosAriaNode::sonarConnectCb,this),
      boost::bind(&RosAriaNode::sonarDisconnectCb, this));
  }
  recharge_state_pub = n.advertise<std_msgs::Int8>("battery_recharge_state", 5, true /*latch*/ );
  recharge_state.data = -2;
  state_of_charge_pub = n.advertise<std_msgs::Float32>("battery_state_of_charge", 100);

  motors_state_pub = n.advertise<std_msgs::Bool>("motors_state", 5, true /*latch*/ );
  motors_state.data = false;
  published_motors_state = false;
 
  // subscribe to services
  cmdvel_sub = n.subscribe( "cmd_vel", 1, (boost::function <void(const geometry_msgs::TwistConstPtr&)>)
    boost::bind(&RosAriaNode::cmdvel_cb, this, _1 ));

  // advertise enable/disable services
  enable_srv = n.advertiseService("enable_motors", &RosAriaNode::enable_motors_cb, this);
  disable_srv = n.advertiseService("disable_motors", &RosAriaNode::disable_motors_cb, this);
 
  veltime = ros::Time::now();
  sonar_tf_timer = n.createTimer(ros::Duration(0.033), &RosAriaNode::sonarCallback, this);
  sonar_tf_timer.stop();

  dynamic_reconfigure_server->setCallback(boost::bind(&RosAriaNode::dynamic_reconfigureCB, this, _1, _2));

  // callback will  be called by ArRobot background processing thread for every SIP data packet received from robot
  robot->addSensorInterpTask("ROSPublishingTask", 100, &myPublishCB);

  // Run ArRobot background processing thread
  robot->runAsync(true);

  return 0;
}
int main( int argc, char ** argv )
{
	ros::init( argc, argv, "sonar_driver" );
	if (argc < 3) {
		ROS_ERROR("sonar_driver usage: sudo rosrun sonar_driver sonar_driver PORT BITRATE\n");
		//port 0, bitrate 40000kHz
  		return 1;
  	}

	// Initialize Cheetah parameters
	port = atoi(argv[1]);
	bitrate = atoi(argv[2]); //kHz

	// Open the device
	handle = ch_open(port);
	if (handle <= 0) {
		ROS_ERROR("Unable to open Cheetah device on port %d (Error code = %d: %s)", port, handle, ch_status_string(handle));
	  	exit(1);
	}
	ROS_INFO("Opened Cheetah device on port %d", port);
	ROS_INFO("Host interface is %s", (ch_host_ifce_speed(handle)) ? "high speed" : "full speed");

	// Configure SPI subsystem
	ch_spi_configure(handle, CH_SPI_POL_RISING_FALLING, CH_SPI_PHASE_SETUP_SAMPLE, CH_SPI_BITORDER_MSB, 0x0);
	ROS_INFO("SPI configuration set to mode %d, %s shift, SS[2:0] active low", mode, "MSB");

	// Power the target using the Cheetah's 5V power supply
	ch_target_power(handle, CH_TARGET_POWER_ON);
	ch_sleep_ms(100);

	// Set the bitrate
	bitrate = ch_spi_bitrate(handle, bitrate);
	ROS_INFO("Bitrate set to %d kHz", bitrate);

  
	// Make a simple queue to assert OE
	ch_spi_queue_clear(handle);
	ch_spi_queue_oe(handle, 1);
	ch_spi_batch_shift(handle, 0, 0);

	// Queue the batch
	ROS_INFO("Beginning to queue SPI packets...");
	ch_spi_queue_clear(handle);
	for (int i = 0; i < DATA_BLOCK_SIZE; ++i) {
		// Convert Slave 1
	 	ch_spi_queue_ss(handle, 0xF);
	 	ch_spi_queue_array(handle, 2, data_out);
	 	ch_spi_queue_ss(handle, 0xE);

	 	// Convert Slave 2
	 	ch_spi_queue_ss(handle, 0xF);
	 	ch_spi_queue_array(handle, 2, data_out);
	 	ch_spi_queue_ss(handle, 0xD);

	 	// Convert Slave 3
	 	ch_spi_queue_ss(handle, 0xF);
	 	ch_spi_queue_array(handle, 2, data_out);
	 	ch_spi_queue_ss(handle, 0xB);
	}
	ROS_INFO("Finished queueing packets\n");

	// Open output filestreams
	std::ofstream file1 ("1_adc_samples.txt");
	std::ofstream file2 ("2_adc_samples.txt");
	std::ofstream file3 ("3_adc_samples.txt");

	int batch_cnt = 1; //count number of sample batches
	double elapsed;
	s64    start;

	while( ros::ok() ) {
		// Submit the batch and collect data
		ROS_INFO("Collecting data from batch %d...", batch_cnt);
		start = _timeMillis();
   		ch_spi_async_submit(handle);
    		ret = ch_spi_async_collect(handle, TX_LENGTH, data_in);
		elapsed = ((double)(_timeMillis() - start)) / 1000;
		ROS_INFO("collected %d bytes from batch #%04d in %.4lf seconds\n", ret, batch_cnt, elapsed);
		if (ret < 0)  ROS_INFO("status error: %s\n", ch_status_string(ret));
		batch_cnt++;

		// Process raw data for the 12-bit ADC's, and write data to text files
    		int data_idx = 0;
		if ( file1.is_open() && file2.is_open() && file3.is_open() ) {
    			for (int j = 0; j < TX_LENGTH; j += 6) {
    				// SS3 Data
		    		input = (data_in[j] << 8) + data_in[j+1];
		    		valid_data_point = (input & 0x3ffc) >> 2;
		    		if(valid_data_point >= 0x0800)	valid_data_point = valid_data_point - 0x1000; //convert 2's comp to signed
		    		data3[data_idx] = valid_data_point;
				file3 << data3[data_idx] << ",";
	
		    		// SS1 Data
		    		input = (data_in[j+2] << 8) + data_in[j+3];
		    		valid_data_point = (input & 0x3ffc) >> 2;
		    		if(valid_data_point >= 0x0800)	valid_data_point = valid_data_point - 0x1000;
		    		data1[data_idx] = valid_data_point;
				file1 << data1[data_idx] << ",";
	
		    		// SS2 Data
		    		input = (data_in[j+4] << 8) + data_in[j+5];
		    		valid_data_point = (input & 0x3ffc) >> 2;
		    		if(valid_data_point >= 0x0800)	valid_data_point = valid_data_point - 0x1000;
		    		data2[data_idx] = valid_data_point;
				file2 << data2[data_idx] << ",";
		    		++data_idx;
    			}
		}
		else std::cout << "Error opening output filestream!" << std::endl;
	}
bool Subscription::negotiateConnection(const std::string& xmlrpc_uri)
{


  XmlRpcValue tcpros_array, protos_array, params;
  XmlRpcValue udpros_array;
  ros::TransportUDPPtr udp_transport;
  int protos = 0;
  ros::V_string transports = transport_hints_.getTransports();
  if (transports.empty())
  {
    transport_hints_.reliable();
    transports = transport_hints_.getTransports();
  }
  for (ros::V_string::const_iterator it = transports.begin();
       it != transports.end();
       ++it)
  {
    if (*it == "UDP")
    {
      int max_datagram_size = transport_hints_.getMaxDatagramSize();
      udp_transport = ros::TransportUDPPtr(new ros::TransportUDP(&ros::PollManager::instance()->getPollSet()));
      if (!max_datagram_size)
        max_datagram_size = udp_transport->getMaxDatagramSize();
      udp_transport->createIncoming(0, false);
      udpros_array[0] = "UDPROS";
      M_string m;
      m["topic"] = getName();
      m["md5sum"] = md5sum();
      m["callerid"] = ros::this_node::getName();
      m["type"] = datatype();
      boost::shared_array<uint8_t> buffer;
      uint32_t len;
      ros::Header::write(m, buffer, len);
      XmlRpcValue v(buffer.get(), len);
      udpros_array[1] = v;
      udpros_array[2] = ros::network::getHost();
      udpros_array[3] = udp_transport->getServerPort();
      udpros_array[4] = max_datagram_size;

      protos_array[protos++] = udpros_array;
    }
    else if (*it == "TCP")
    {
      tcpros_array[0] = std::string("TCPROS");
      protos_array[protos++] = tcpros_array;
    }
    else
    {
      ROS_WARN("Unsupported transport type hinted: %s, skipping", it->c_str());
    }
  }
  params[0] = ros::this_node::getName();
  params[1] = name_;
  params[2] = protos_array;
  std::string peer_host;
  uint32_t peer_port;
  if (!ros::network::splitURI(xmlrpc_uri, peer_host, peer_port))
  {
    ROS_ERROR("Bad xml-rpc URI: [%s]", xmlrpc_uri.c_str());
    return false;
  }

  XmlRpcClient* c = new XmlRpcClient(peer_host.c_str(),
                                                     peer_port, "/");
 // if (!c.execute("requestTopic", params, result) || !g_node->validateXmlrpcResponse("requestTopic", result, proto))

  // Initiate the negotiation.  We'll come back and check on it later.
  if (!c->executeNonBlock("requestTopic", params))
  {
    ROSCPP_LOG_DEBUG("Failed to contact publisher [%s:%d] for topic [%s]",
              peer_host.c_str(), peer_port, name_.c_str());
    delete c;
    if (udp_transport)
    {
      udp_transport->close();
    }

    return false;
  }

  ROSCPP_LOG_DEBUG("Began asynchronous xmlrpc connection to [%s:%d]", peer_host.c_str(), peer_port);

  // The PendingConnectionPtr takes ownership of c, and will delete it on
  // destruction.
  PendingConnectionPtr conn(new PendingConnection(c, udp_transport, shared_from_this(), xmlrpc_uri));

//ROS_INFO("add connection to xmlrpcmanager");

  XMLRPCManager::instance()->addASyncConnection(conn);
  // Put this connection on the list that we'll look at later.
  {
    boost::mutex::scoped_lock pending_connections_lock(pending_connections_mutex_);
    pending_connections_.insert(conn);
  }

  return true;
}
Beispiel #19
0
    bool LCM2ROSControl::initRequest(hardware_interface::RobotHW* robot_hw, 
                             ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh, 
                             std::set<std::string>& claimed_resources)
    {
        // check if construction finished cleanly
        if (state_ != CONSTRUCTED){
          ROS_ERROR("Cannot initialize this controller because it failed to be constructed");
          return false;
        }

        // setup LCM (todo: move to constructor? how to propagate an error then?)
        lcm_ = boost::shared_ptr<lcm::LCM>(new lcm::LCM);
        if (!lcm_->good())
        {
          std::cerr << "ERROR: lcm is not good()" << std::endl;
          return false;
        }
        handler_ = std::shared_ptr<LCM2ROSControl_LCMHandler>(new LCM2ROSControl_LCMHandler(*this));
        
        // get a pointer to the effort interface
        hardware_interface::EffortJointInterface* effort_hw = robot_hw->get<hardware_interface::EffortJointInterface>();
        if (!effort_hw)
        {
          ROS_ERROR("This controller requires a hardware interface of type hardware_interface::EffortJointInterface.");
          return false;
        }

        effort_hw->clearClaims();
        const std::vector<std::string>& effortNames = effort_hw->getNames();
        // initialize command buffer for each joint we found on the HW
        for(unsigned int i=0; i<effortNames.size(); i++)
        {
          effortJointHandles[effortNames[i]] = effort_hw->getHandle(effortNames[i]);
          latest_commands[effortNames[i]] = drc::joint_command_t();
          latest_commands[effortNames[i]].joint_name = effortNames[i];
          latest_commands[effortNames[i]].position = 0.0;
          latest_commands[effortNames[i]].velocity = 0.0;
          latest_commands[effortNames[i]].effort = 0.0;
          latest_commands[effortNames[i]].k_q_p = 0.0;
          latest_commands[effortNames[i]].k_q_i = 0.0;
          latest_commands[effortNames[i]].k_qd_p = 0.0;
          latest_commands[effortNames[i]].k_f_p = 0.0;
          latest_commands[effortNames[i]].ff_qd = 0.0;
          latest_commands[effortNames[i]].ff_qd_d = 0.0;
          latest_commands[effortNames[i]].ff_f_d = 0.0;
          latest_commands[effortNames[i]].ff_const = 0.0;
        }

        auto effort_hw_claims = effort_hw->getClaims();
        claimed_resources.insert(effort_hw_claims.begin(), effort_hw_claims.end());
        effort_hw->clearClaims();

        // get a pointer to the imu interface
        hardware_interface::ImuSensorInterface* imu_hw = robot_hw->get<hardware_interface::ImuSensorInterface>();
        if (!imu_hw)
        {
          ROS_ERROR("This controller requires a hardware interface of type hardware_interface::ImuSensorInterface.");
          return false;
        }

        imu_hw->clearClaims();
        const std::vector<std::string>& imuNames = imu_hw->getNames();
        for(unsigned int i=0; i<imuNames.size(); i++)
        {
             imuSensorHandles[imuNames[i]] = imu_hw->getHandle(imuNames[i]);
        }

        auto imu_hw_claims = imu_hw->getClaims();
        claimed_resources.insert(imu_hw_claims.begin(), imu_hw_claims.end());
        imu_hw->clearClaims();

        hardware_interface::ForceTorqueSensorInterface* forceTorque_hw = robot_hw->get<hardware_interface::ForceTorqueSensorInterface>();
        if (!forceTorque_hw)
        {
          ROS_ERROR("This controller requires a hardware interface of type hardware_interface::EffortJointInterface.");
          return false;
        }

        // get pointer to forcetorque interface
        forceTorque_hw->clearClaims();
        const std::vector<std::string>& forceTorqueNames = forceTorque_hw->getNames();
        for(unsigned int i=0; i<forceTorqueNames.size(); i++)
        {
             forceTorqueHandles[forceTorqueNames[i]] = forceTorque_hw->getHandle(forceTorqueNames[i]);
        }
        auto forceTorque_hw_claims = forceTorque_hw->getClaims();
        claimed_resources.insert(forceTorque_hw_claims.begin(), forceTorque_hw_claims.end());
        forceTorque_hw->clearClaims();

        // success
        state_ = INITIALIZED;
        ROS_INFO("LCM2ROSCONTROL ON\n");
        return true;
    }
  bool SerialInterface::getPacket (char *spacket, unsigned char &packet_type, unsigned short &packet_crc,
                                   unsigned short &packet_size)
  {
    char stoken[4];
    char ssize[2];
    char stype[1];
    char scrc[2];
    int bytes;

    int i;

    ROS_DEBUG ("  SerialInterface::getPacket()");
    // get beginning (">*>")
    stoken[0] = '\0';
    stoken[1] = '\0';
    stoken[2] = '\0';
    stoken[3] = '\0';

    wait(3);
    i = read (dev_,stoken, 3);
    if (i == 0 || strncmp (stoken, ">*>", 3) != 0)
    {
      ROS_DEBUG ("    dev: %zd", (size_t) dev_);
      ROS_ERROR ("    Error Reading Packet Header: %s", strerror (errno));
      ROS_ERROR ("    Read (%d): %s", i, stoken);
      //ROS_BREAK();
      //while (read (dev_,stoken, 1) != 0) {}
      flush ();
      return false;
    }
    serialport_bytes_rx_ += 3;
    ROS_DEBUG ("    Packet Header OK");

    // get packet size
    wait(2);
    i = read (dev_,ssize, 2);
    if (i == 0)
    {
      ROS_ERROR ("Error Reading Packet Size: %s", strerror (errno));
      flush ();
      return false;
    }
    serialport_bytes_rx_ += 2;
    memcpy (&packet_size, ssize, sizeof (packet_size));
    //ROS_DEBUG ("Packet size: %d", packet_size);

    // get packet type
    wait(1);
    i = read (dev_, stype, 1);
    if (i == 0)
      return false;
    serialport_bytes_rx_ += 1;
    memcpy (&packet_type, stype, sizeof (packet_type));
    //ROS_DEBUG ("Packet type: %d", packet_type);

    // get packet
    wait(packet_size);
    i = read (dev_, spacket, packet_size);
    if (i == 0)
      return false;
    serialport_bytes_rx_ += packet_size;
    //ROS_DEBUG ("Packet string: ok");

    // get packet crc
    wait(2);
    i = read (dev_, scrc, 2);
    if (i == 0)
      return false;
    serialport_bytes_rx_ += sizeof (scrc);
    memcpy (&packet_crc, scrc, sizeof (packet_crc));
    //ROS_DEBUG ("Packet crc: %d", packet_crc);

    // get closing ("<#<")
    wait(3);
    i = read (dev_, stoken, 3);
    if (i == 0 || strncmp (stoken, "<#<", 3) != 0)
    {
      ROS_ERROR ("Error Reading Packet Footer: %s", strerror (errno));
      ROS_DEBUG ("Read (%d): %s", i, stoken);
      while (read (dev_, stoken, 1) != 0)
      {
        stoken[1] = '\0';
        ROS_DEBUG ("%s", stoken);
      }
      flush ();
      drain ();
      ROS_DEBUG ("Packet Footer Corrupt!!");
      return false;
    }
    serialport_bytes_rx_ += 3;
    //ROS_DEBUG ("Packet Footer OK");

    return true;
  }
int main(int argc, char **argv)
{
	char** _argv = NULL;
	int _argc = 0;
	int Narg=0;
	int cuentaObs=0;
	float Lx=0.0, Ly=0.0;
    float periodo, f;
	int k=0;
	std::string fileName;

    Narg=6;

    if (argc>Narg)
	{
        Ly = atof(argv[1]);
        Lx = atof(argv[2]);
        nCeldas_i = atoi(argv[3]);
        nCeldas_j = atoi(argv[4]);
        fileName = argv[5];
        cantidadObstaculos = atoi(argv[6]);
	} else{
        ROS_ERROR("Nodo 5: Indique argumentos completos!\n");
        return (0);
     }

//--- Inicializacion de nodo de ROS
    ros::init(_argc,_argv,"Nodo5_ConstruccionDeMapa");
	ROS_INFO("Nodo5_ConstruccionDeMapa just started");

    ros::NodeHandle node;
//-- Topicos susbcritos y publicados
    ros::Subscriber subInfo1=node.subscribe("/vrep/info",100,infoCallback);
    ros::Subscriber subInfo2=node.subscribe("UbicacionRobot",100,ubicacionRobCallback);
    ros::Subscriber subInfo3=node.subscribe("Plan",100,ajusteCallback);
    chatter_pub = node.advertise<camina::InfoMapa>("GraficaMapa", 100);
    //Clientes y Servicios
//    client_EspacioTrabajo=node.serviceClient<camina::EspacioTrabajoParametros>("EspacioTrabajo");

//--- Inicializacion de variables
    LongitudCeldaY = infoMapa.tamanoCelda_i = Ly/(float) nCeldas_i;
    LongitudCeldaX = infoMapa.tamanoCelda_j = Lx/(float) nCeldas_j;
//    infoMapa.tamanoCelda_i = Ly/(float) nCeldas_i;
//    infoMapa.tamanoCelda_j = Lx/(float) nCeldas_j;
//-- Inicializacion de mensaje para graficar
    infoMapa.nCeldas_i=nCeldas_i;
    infoMapa.nCeldas_j=nCeldas_j;
    infoMapa.cantidadObstaculos = cantidadObstaculos;
    for (k=0; k<cantidadObstaculos;k++) {
        infoMapa.coordenadaObstaculo_i.push_back(0);
        infoMapa.coordenadaObstaculo_j.push_back(0);
    }
    for (k=0; k<Npatas;k++) {
        infoMapa.coordenadaPata_i.push_back(0);
        infoMapa.coordenadaPata_j.push_back(0);
        infoMapa.pataApoyo.push_back(0);
    }
    // Inicializacion de apuntadores
    p_ij = ij;
    // Inicializacion de matriz nula
    for(int i=0;i<nCeldas_i;i++){
        for(int j=0;j<nCeldas_j;j++){
            matrizMapa[i][j]=-1;
            bool_matrizMapa[i][j]=false;
        }
    }

    cuentaObs = Construye_matrizMapa(fileName, nCeldas_i, nCeldas_j);
    printf("obstaculos recibidos: %d, obstaculos contados: %d\n",cantidadObstaculos,cuentaObs);
//--- Inicializacion de grafica
//    IniciaGrafica();
//--- Inicializacion de archivo de salida
    fp1 = fopen("../fuerte_workspace/sandbox/TesisMaureen/ROS/camina/datos/SalidaMapa_robot.txt","w+");
    fp2 = fopen("../fuerte_workspace/sandbox/TesisMaureen/ROS/camina/datos/SalidaMapa_ajuste.txt","w+");
//--- Ciclo de ROS
//	periodo=1.5;
//    f=1/periodo;
//    ros::Rate loop_rate(f);  //Frecuencia [Hz]
//    for (int i=0; i<3; i++) loop_rate.sleep();
    chatter_pub.publish(infoMapa);

	while (ros::ok() && simulationRunning)
	{
//        for (int k=0; k<Npatas;k++) {
//            //-- Punto 1
//            transformacion_yxTOij(p_ij,EspacioTrabajoPata_y[k][0],EspacioTrabajoPata_x[k][0]);
//            EspacioTrabajoPatas_ij[k][0]=ij[0];
//            EspacioTrabajoPatas_ij[k][1]=ij[1];
//            //-- Punto 2
//            transformacion_yxTOij(p_ij,EspacioTrabajoPata_y[k][1],EspacioTrabajoPata_x[k][1]);
//            EspacioTrabajoPatas_ij[k][2]=ij[0];
//            EspacioTrabajoPatas_ij[k][3]=ij[1];
//            //-- Punto 3
//            transformacion_yxTOij(p_ij,EspacioTrabajoPata_y[k][2],EspacioTrabajoPata_x[k][2]);
//            EspacioTrabajoPatas_ij[k][4]=ij[0];
//            EspacioTrabajoPatas_ij[k][5]=ij[1];
//            //-- Punto 4
//            transformacion_yxTOij(p_ij,EspacioTrabajoPata_y[k][3],EspacioTrabajoPata_x[k][3]);
//            EspacioTrabajoPatas_ij[k][6]=ij[0];
//            EspacioTrabajoPatas_ij[k][7]=ij[1];
//        }
//    //-- Funcion para grafica de cuerpo
//        FuncionGrafica_Cuerpo();

//        chatter_pub.publish(infoMapa);
        ros::spinOnce();
//        loop_rate.sleep();
	}
	fclose(fp1); fclose(fp2);
    ROS_INFO("AdiosGrafica!");
    ros::shutdown();
    return 0;
}
  bool SerialInterface::getPackets (Telemetry * telemetry)
  {
    flush ();
    ROS_DEBUG ("SerialInterface::getPackets");
    char cmd[16];
    char spacket[1024];
    unsigned char packet_type;
    unsigned short packet_crc;
    unsigned short packet_size;
    unsigned int i;
    bool result = false;
    ros::Time packetTime;

    ROS_DEBUG ("  Requesting %04x %zd packets", (short) telemetry->requestPackets_.to_ulong (),
              telemetry->requestPackets_.count ());
    sprintf (cmd, ">*>p%c", (short) telemetry->requestPackets_.to_ulong ());
    output (cmd, 6);

    for (i = 0; i < telemetry->requestPackets_.count (); i++)
    {
      packetTime = ros::Time::now();  // Presumes that the AutoPilot is grabbing the data for each packet
                                      // immediately prior to each packet being sent, as opposed to gathering
                                      // all the data at once and then sending it all. Either way is a guess
                                      // unless we get some info from AscTec one way or the other..
      bool read_result = getPacket (spacket, packet_type, packet_crc, packet_size);

      if (read_result)
      {
        ROS_DEBUG ("  Read successful: type = %d, crc = %d", packet_type, packet_crc);

        if (packet_type == Telemetry::PD_LLSTATUS)
        {
          ROS_DEBUG ("  Packet type is LL_STATUS");
          memcpy (&telemetry->LL_STATUS_, spacket, packet_size);
          telemetry->timestamps_[RequestTypes::LL_STATUS] = packetTime;
          if (crc_valid (packet_crc, &telemetry->LL_STATUS_, sizeof (packet_size)))
          {
            result = true;
          }
          //telemetry->dumpLL_STATUS();
        }
        else if (packet_type == Telemetry::PD_IMURAWDATA)
        {
          ROS_DEBUG ("  Packet type is IMU_RAWDATA");
          memcpy (&telemetry->IMU_RAWDATA_, spacket, packet_size);
          telemetry->timestamps_[RequestTypes::IMU_RAWDATA] = packetTime;
          if (crc_valid (packet_crc, &telemetry->IMU_RAWDATA_, packet_size))
          {
            result = true;
          }
          //telemetry->dumpIMU_RAWDATA();
        }
        else if (packet_type == Telemetry::PD_IMUCALCDATA)
        {
          ROS_DEBUG ("  Packet type is IMU_CALCDATA");
          memcpy (&telemetry->IMU_CALCDATA_, spacket, packet_size);
          telemetry->timestamps_[RequestTypes::IMU_CALCDATA] = packetTime;
          if (crc_valid (packet_crc, &telemetry->IMU_CALCDATA_, packet_size))
          {
            result = true;
          }
          //telemetry->dumpIMU_CALCDATA();
        }
        else if (packet_type == Telemetry::PD_RCDATA)
        {
          ROS_DEBUG ("  Packet type is RC_DATA");
          memcpy (&telemetry->RC_DATA_, spacket, packet_size);
          telemetry->timestamps_[RequestTypes::RC_DATA] = packetTime;
          if (crc_valid (packet_crc, &telemetry->RC_DATA_, packet_size))
          {
            result = true;
          }
          //telemetry->dumpRC_DATA();
        }
        else if (packet_type == Telemetry::PD_CTRLOUT)
        {
          ROS_DEBUG ("  Packet type is CONTROLLER_OUTPUT");
          memcpy (&telemetry->CONTROLLER_OUTPUT_, spacket, packet_size);
          telemetry->timestamps_[RequestTypes::CONTROLLER_OUTPUT] = packetTime;
          if (crc_valid (packet_crc, &telemetry->CONTROLLER_OUTPUT_, packet_size))
          {
            result = true;
          }
          //telemetry->dumpCONTROLLER_OUTPUT();
        }
        else if (packet_type == Telemetry::PD_GPSDATA)
        {
          ROS_DEBUG ("  Packet type is GPS_DATA");
          memcpy (&telemetry->GPS_DATA_, spacket, packet_size);
          telemetry->timestamps_[RequestTypes::GPS_DATA] = packetTime;
          if (crc_valid (packet_crc, &telemetry->GPS_DATA_, packet_size))
          {
            result = true;
          }
          //telemetry->dumpGPS_DATA();
        }
        else if (packet_type == Telemetry::PD_GPSDATAADVANCED)
        {
          ROS_DEBUG ("  Packet type is GPS_DATA_ADVANCED");
          memcpy (&telemetry->GPS_DATA_ADVANCED_, spacket, packet_size);
          telemetry->timestamps_[RequestTypes::GPS_DATA_ADVANCED] = packetTime;
          if (crc_valid (packet_crc, &telemetry->GPS_DATA_ADVANCED_, packet_size))
          {
            result = true;
          }
          //telemetry->dumpGPS_DATA_ADVANCED();
        }
        else
        {
          ROS_ERROR ("  Packet type (%#2x) is UNKNOWN", packet_type);
        }
      }
      else
      {
        // failed read
        ROS_ERROR ("  Read failed");
        break;
      }
    }
    ioctl(dev_,FIONREAD,&i);
    if (i != 0)
    {
      ROS_ERROR ("  Unexpected Data: Flushing receive buffer");
      flush();
    }
    return result;
  }
FootstepNavigation::FootstepNavigation()
: ivIdFootRight("/r_sole"),
  ivIdFootLeft("/l_sole"),
  ivIdMapFrame("map"),
  ivExecutingFootsteps(false),
  ivFootstepsExecution("footsteps_execution", true),
  ivExecutionShift(2),
  ivControlStepIdx(-1),
  ivResetStepIdx(0)
{
  // private NodeHandle for parameters and private messages (debug / info)
  ros::NodeHandle nh_private("~");
  ros::NodeHandle nh_public;

  // service
  ivFootstepSrv =
    nh_public.serviceClient<humanoid_nav_msgs::StepTargetService>(
      "footstep_srv");
  ivClipFootstepSrv =
    nh_public.serviceClient<humanoid_nav_msgs::ClipFootstep>(
      "clip_footstep_srv");

  // subscribers
  ivGridMapSub =
    nh_public.subscribe<nav_msgs::OccupancyGrid>(
      "map", 1, &FootstepNavigation::mapCallback, this);
  ivGoalPoseSub =
    nh_public.subscribe<geometry_msgs::PoseStamped>(
      "goal", 1, &FootstepNavigation::goalPoseCallback, this);

  // read parameters from config file:
  nh_private.param("rfoot_frame_id", ivIdFootRight, ivIdFootRight);
  nh_private.param("lfoot_frame_id", ivIdFootLeft, ivIdFootLeft);

  nh_private.param("accuracy/footstep/x", ivAccuracyX, 0.01);
  nh_private.param("accuracy/footstep/y", ivAccuracyY, 0.01);
  nh_private.param("accuracy/footstep/theta", ivAccuracyTheta, 0.1);

  nh_private.param("accuracy/cell_size", ivCellSize, 0.005);
  nh_private.param("accuracy/num_angle_bins", ivNumAngleBins, 128);

  nh_private.param("forward_search", ivForwardSearch, false);

  nh_private.param("feedback_frequency", ivFeedbackFrequency, 5.0);
  nh_private.param("safe_execution", ivSafeExecution, true);

  nh_private.param("foot/max/step/x", ivMaxStepX, 0.07);
  nh_private.param("foot/max/step/y", ivMaxStepY, 0.15);
  nh_private.param("foot/max/step/theta", ivMaxStepTheta, 0.3);
  nh_private.param("foot/max/inverse/step/x", ivMaxInvStepX, -0.03);
  nh_private.param("foot/max/inverse/step/y", ivMaxInvStepY, 0.09);
  nh_private.param("foot/max/inverse/step/theta", ivMaxInvStepTheta, -0.01);

  // step range
  XmlRpc::XmlRpcValue step_range_x;
  XmlRpc::XmlRpcValue step_range_y;
  nh_private.getParam("step_range/x", step_range_x);
  nh_private.getParam("step_range/y", step_range_y);
  if (step_range_x.getType() != XmlRpc::XmlRpcValue::TypeArray)
    ROS_ERROR("Error reading footsteps/x from config file.");
  if (step_range_y.getType() != XmlRpc::XmlRpcValue::TypeArray)
    ROS_ERROR("Error reading footsteps/y from config file.");
  if (step_range_x.size() != step_range_y.size())
  {
    ROS_ERROR("Step range points have different size. Exit!");
    exit(2);
  }
  // create step range
  ivStepRange.clear();
  ivStepRange.reserve(step_range_x.size());
  double x, y;
  for (int i = 0; i < step_range_x.size(); ++i)
  {
    x = (double)step_range_x[i];
    y = (double)step_range_y[i];
    ivStepRange.push_back(std::pair<double, double>(x, y));
  }
  // insert first point again at the end!
  ivStepRange.push_back(ivStepRange[0]);
}
  /*!
   * \brief Gets parameters from the ROS parameter server and configures the powercube_chain.
   */
  void getROSParameters()
  {
    // get CanModule
    std::string CanModule;
    if (n_.hasParam("can_module"))
    {
      n_.getParam("can_module", CanModule);
    }
    else
    {
      ROS_ERROR("Parameter can_module not set, shutting down node...");
      n_.shutdown();
    }

    // get CanDevice
    std::string CanDevice;
    if (n_.hasParam("can_device"))
    {
      n_.getParam("can_device", CanDevice);
    }
    else
    {
      ROS_ERROR("Parameter can_device not set, shutting down node...");
      n_.shutdown();
    }

    // get CanBaudrate
    int CanBaudrate;
    if (n_.hasParam("can_baudrate"))
    {
      n_.getParam("can_baudrate", CanBaudrate);
    }
    else
    {
      ROS_ERROR("Parameter can_baudrate not set, shutting down node...");
      n_.shutdown();
    }

    // get modul ids
    XmlRpc::XmlRpcValue ModulIDsXmlRpc;
    std::vector<int> ModulIDs;
    if (n_.hasParam("modul_ids"))
    {
      n_.getParam("modul_ids", ModulIDsXmlRpc);
    }
    else
    {
      ROS_ERROR("Parameter modul_ids not set, shutting down node...");
      n_.shutdown();
    }
    ModulIDs.resize(ModulIDsXmlRpc.size());
    for (int i = 0; i < ModulIDsXmlRpc.size(); i++)
    {
      ModulIDs[i] = (int)ModulIDsXmlRpc[i];
    }

    // init parameters
    pc_params_->Init(CanModule, CanDevice, CanBaudrate, ModulIDs);

    // get joint names
    XmlRpc::XmlRpcValue JointNamesXmlRpc;
    std::vector<std::string> JointNames;
    if (n_.hasParam("joint_names"))
    {
      n_.getParam("joint_names", JointNamesXmlRpc);
    }
    else
    {
      ROS_ERROR("Parameter joint_names not set, shutting down node...");
      n_.shutdown();
    }
    JointNames.resize(JointNamesXmlRpc.size());
    for (int i = 0; i < JointNamesXmlRpc.size(); i++)
    {
      JointNames[i] = (std::string)JointNamesXmlRpc[i];
    }
    // check dimension with with DOF
    if ((int)JointNames.size() != pc_params_->GetDOF())
    {
      ROS_ERROR("Wrong dimensions of parameter joint_names, shutting down node...");
      n_.shutdown();
    }
    pc_params_->SetJointNames(JointNames);

    // get max accelerations
    XmlRpc::XmlRpcValue MaxAccelerationsXmlRpc;
    std::vector<double> MaxAccelerations;
    if (n_.hasParam("max_accelerations"))
    {
      n_.getParam("max_accelerations", MaxAccelerationsXmlRpc);
    }
    else
    {
      ROS_ERROR("Parameter max_accelerations not set, shutting down node...");
      n_.shutdown();
    }
    MaxAccelerations.resize(MaxAccelerationsXmlRpc.size());
    for (int i = 0; i < MaxAccelerationsXmlRpc.size(); i++)
    {
      MaxAccelerations[i] = (double)MaxAccelerationsXmlRpc[i];
    }
    // check dimension with with DOF
    if ((int)MaxAccelerations.size() != pc_params_->GetDOF())
    {
      ROS_ERROR("Wrong dimensions of parameter max_accelerations, shutting down node...");
      n_.shutdown();
    }
    pc_params_->SetMaxAcc(MaxAccelerations);

    // get horizon
    double Horizon;
    if (n_.hasParam("horizon"))
    {
      n_.getParam("horizon", Horizon);
    }
    else
    {
      Horizon = 0.025; //Hz
      ROS_WARN("Parameter horizon not available, setting to default value: %f sec", Horizon);
    }
    pc_ctrl_->setHorizon(Horizon);
  }
void GazeboRosControllerManager::LoadThread()
{
  // Get then name of the parent model
  std::string modelName = this->sdf->GetParent()->GetValueString("name");

  // Get the world name.
  this->world = this->parent_model_->GetWorld();

  // Error message if the model couldn't be found
  if (!this->parent_model_)
    ROS_ERROR("parent model in NULL.");



  if (getenv("CHECK_SPEEDUP"))
  {
    wall_start_ = this->world->GetRealTime().Double();
    sim_start_  = this->world->GetSimTime().Double();
  }

  // check update rate against world physics update rate
  // should be equal or higher to guarantee the wrench applied is not "diluted"
  // if (this->updatePeriod > 0 &&
  //   (this->world->GetPhysicsEngine()->GetUpdateRate() >
  //   1.0/this->updatePeriod))
  //   ROS_ERROR("gazebo_ros_controller_manager update rate is less than"
  //     " physics update rate, wrench applied will be diluted"
  //     " (applied intermittently)");

  // get parameter name
  this->robotNamespace = "";
  if (this->sdf->HasElement("robotNamespace"))
    this->robotNamespace =
      this->sdf->GetElement("robotNamespace")->GetValueString();

  this->robotParam = "robot_description";
  if (this->sdf->HasElement("robotParam"))
    this->robotParam =
      this->sdf->GetElement("robotParam")->GetValueString();

  this->robotParam = this->robotNamespace+"/" + this->robotParam;

  // Exit if no ROS
  if (!ros::isInitialized())
  {
    gzerr << "Not loading plugin since ROS hasn't been "
          << "properly initialized.  Try starting gazebo with ros plugin:\n"
          << "  gazebo -s libgazebo_ros_api_plugin.so\n";
    return;
  }

  /* Init ROS
  if (!ros::isInitialized())
  {
    int argc = 0;
    char** argv = NULL;
    ros::init( argc, argv, "gazebo", ros::init_options::NoSigintHandler);
    gzwarn << "should start ros::init in simulation by using the"
           << " system plugin\n";
  }*/

  this->rosnode_ = new ros::NodeHandle(this->robotNamespace);
  ROS_INFO("starting gazebo_ros_controller_manager plugin in ns: %s",
    this->robotNamespace.c_str());

  // pr2_etherCAT calls ros::spin(), we'll thread out one spinner
  // here to mimic that
  this->ros_spinner_thread_ = boost::thread(
    boost::bind(&GazeboRosControllerManager::ControllerManagerROSThread, this));

  // load a controller manager, initialize hardware_interface
  this->controller_manager_ = new pr2_controller_manager::ControllerManager(
    &hardware_interface_, *this->rosnode_);
  this->hardware_interface_.current_time_ = ros::Time(
    this->world->GetSimTime().Double());

  // hardcoded to minimum of 1ms on start up
  if (this->hardware_interface_.current_time_ < ros::Time(0.001))
    this->hardware_interface_.current_time_ == ros::Time(0.001);

  this->rosnode_->param("gazebo/start_robot_calibrated",
    this->calibration_status_, true);

  // Read urdf from ros parameter server then
  // setup actuators and mechanism control node.
  // This call will block if ROS is not properly initialized.
  if (!LoadControllerManagerFromURDF())
  {
    ROS_ERROR("Error parsing URDF in gazebo controller manager plugin,"
              " plugin not active.\n");
    return;
  }

  // Initializes the fake state (for running the transmissions backwards).
  this->virtual_mechanism_state_ =
    new pr2_mechanism_model::RobotState(&this->controller_manager_->model_);

  // The gazebo joints and mechanism joints should match up.
  for (unsigned int i = 0;
       i < this->controller_manager_->state_->joint_states_.size(); ++i)
  {
    std::string joint_name =
      this->controller_manager_->state_->joint_states_[i].joint_->name;

    // fill in gazebo joints pointer
    gazebo::physics::JointPtr joint =
      this->parent_model_->GetJoint(this->parent_model_->GetName() +
      "::" + joint_name);

    if (joint)
    {
      this->gazebo_joints_.push_back(joint);
    }
    else
    {
      ROS_WARN("A Mechanism Controlled joint named [%s] is not found"
        " in gazebo model[%s].\n",
        joint_name.c_str(), this->parent_model_->GetName().c_str());
      this->gazebo_joints_.push_back(gazebo::physics::JointPtr());
    }
  }
  assert(this->gazebo_joints_.size() ==
    this->virtual_mechanism_state_->joint_states_.size());

#ifdef USE_CBQ
  // start custom queue for controller manager
  this->controller_manager_callback_queue_thread_ = boost::thread(boost::bind(
    &GazeboRosControllerManager::ControllerManagerQueueThread, this));
#endif

  // If all previous steps are successful, start the controller manager
  // plugin updates
  this->updateConnection = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&GazeboRosControllerManager::UpdateControllerForces, this));
}
  /*!
   * \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);
  }
void HermesMoveArmActionServer::moveArm(const hermes_move_arm_action::MoveArmGoalConstPtr& goal)
{
	// this callback function is executed each time a request (= goal message) comes in for this service server
	ROS_INFO("MoveArm Action Server: Received a request for arm %i.", goal->arm);

	if (goal->goal_position.header.frame_id.compare("/world") != 0)
	{
		ROS_ERROR("The goal position coordinates are not provided in the correct frame. The required frame is '/world' but '%s' was provided.", goal->goal_position.header.frame_id.c_str());
		return;
	}

	// this command sends a feedback message to the caller, here we transfer that the task is completed 25%
//	hermes_move_arm_action::MoveArmFeedback feedback;
//	feedback.percentageDone = 25;
//	move_arm_action_server_.publishFeedback(feedback);

	// move the arm
	// ...

//goal->goal_position.pose.position.x  //(x,y,z) in meters?????
//goal->goal_position.pose.orientation.w //(w,x,y,z) Quaternion in rad
//goal->goal_position.header.frame_id // Reference frame
	tf::Quaternion quaternion;
	tf::quaternionMsgToTF(goal->goal_position.pose.orientation, quaternion);
	tf::Transform goalPos(quaternion, tf::Vector3(goal->goal_position.pose.position.x,goal->goal_position.pose.position.y,goal->goal_position.pose.position.z));


	// Transform goalPos to Robot Reference frame
	hermes_reference_frames_service::HermesFrame::Request req_frames;
	hermes_reference_frames_service::HermesFrame::Response res_frames;

	if(goal->arm == hermes_move_arm_action::MoveArmGoal::LEFTARM) // Depends of arm
		req_frames.frame = hermes_reference_frames_service::HermesFrame::Request::WORLDTLEFTARM;

	else if(goal->arm == hermes_move_arm_action::MoveArmGoal::RIGHTARM) // Depends of arm
		req_frames.frame = hermes_reference_frames_service::HermesFrame::Request::WORLDTRIGHTARM;

	ros::service::call("reference_frames_service", req_frames, res_frames);

	// Transform world to base robot
	tf::Quaternion qua_wTr(res_frames.quaternion[0],res_frames.quaternion[1],res_frames.quaternion[2],res_frames.quaternion[3]);
	tf::Transform wTr(qua_wTr, tf::Vector3(res_frames.position[0],res_frames.position[1],res_frames.position[2]));

	tf::Transform rTobj;
	rTobj = wTr.inverse()*goalPos;


	std::cout << "rTobj: " << std::endl;
	for (int i=0; i<3; ++i)
	{
		std::cout << rTobj.getBasis()[i].getX() << "\t";
		std::cout << rTobj.getBasis()[i].getY() << "\t";
		std::cout << rTobj.getBasis()[i].getZ() <<std::endl;
	}

	std::cout << "XYZ: " << std::endl;
	std::cout << rTobj.getOrigin().getX() <<std::endl;
	std::cout << rTobj.getOrigin().getY() <<std::endl;
	std::cout << rTobj.getOrigin().getZ() <<std::endl;


//	tf::Vector3 rotX=rTobj.getBasis()[0];
//	tf::Vector3 rotY=rTobj.getBasis()[1];
//	tf::Vector3 rotZ=rTobj.getBasis()[2];
//
//
//	KDL::Vector rotXkdl(rotX.getX(),rotX.getY(),rotX.getZ());
//	KDL::Vector rotYkdl(rotY.getX(),rotY.getY(),rotY.getZ());
//	KDL::Vector rotZkdl(rotZ.getX(),rotZ.getY(),rotZ.getZ());
//
//	KDL::Rotation rot(rotXkdl,rotYkdl,rotZkdl);



	//Get init position
	std::vector<float> q_init;

	if(goal->arm == hermes_move_arm_action::MoveArmGoal::LEFTARM)
		hermesinterface.get_leftJoints(q_init);
	else if(goal->arm == hermes_move_arm_action::MoveArmGoal::RIGHTARM)
		hermesinterface.get_rightJoints(q_init);


   // todo: call hermes_arm_kdl ikine service
	hermes_arm_kdl::ikine::Request req_kdl;
	hermes_arm_kdl::ikine::Response res_kdl;
	for (int i=0; i<7; i++)
		req_kdl.jointAngles_init[i] = q_init[i];
	req_kdl.position[0] = rTobj.getOrigin().getX();
	req_kdl.position[1] = rTobj.getOrigin().getY();
	req_kdl.position[2] = rTobj.getOrigin().getZ();

	for (int i=0; i<3; i++)
	{
		req_kdl.rotation[3*i] = rTobj.getBasis()[i].getX();
		req_kdl.rotation[3*i+1] = rTobj.getBasis()[i].getY();
		req_kdl.rotation[3*i+2] = rTobj.getBasis()[i].getZ();
	}
	ros::service::call("arm_kdl_service_ikine_server", req_kdl, res_kdl);

	// Move the arm with res_kdl
	std::vector<float> jointAngles(7);

	for (int i=0; i<7; i++)
		jointAngles[i] = res_kdl.jointAngles[i];

	std::cout << "jointAngles:" << std::endl;
	for (int i=0; i<7; i++)
		std::cout << jointAngles[i] << std::endl;

	//ARMS DON'T MOVE
	if(goal->arm == hermes_move_arm_action::MoveArmGoal::LEFTARM)
		hermesinterface.moveLeftArm(jointAngles);
	else if(goal->arm == hermes_move_arm_action::MoveArmGoal::RIGHTARM)
		hermesinterface.moveRightArm(jointAngles);

	hermes_move_arm_action::MoveArmResult res;
	res.return_value.val = arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS; 	// put in there some error code on errors


	// this sends the response back to the caller
	move_arm_action_server_.setSucceeded(res);

	// if failed, use: move_arm_action_server_.setAborted(res);
}
  /*!
   * \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_)
    {

      PowerCubeCtrl::PC_CTRL_STATUS status;
      std::vector<std::string> errorMessages;
      ROS_WARN("here");
      pc_ctrl_->getStatus(status, errorMessages);
      ROS_WARN("here2");
      std::cout << status << std::endl;

      // @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))
      {
        ROS_ERROR("Skipping command: %s",pc_ctrl_->getErrorMessage().c_str());
        return;
      }

      ROS_DEBUG("Executed velocity command");
    }
    else
    {
      ROS_ERROR("Skipping command: powercubes not initialized");
    }
  }
void StorePuckServer::controlLoop()
{
	double vel_x = 0, vel_y = 0, vel_phi = 0;
	ROS_DEBUG("%s", StorePuckStates::toString(state_).c_str());
	double percentage = 0;
	if (mode_ == store_modes::LASER_SCAN)
	{
		robotino_motion::AlignGoal frontal_alignment, lateral_alignment;
		frontal_alignment.alignment_mode = 8; // FRONT_LASER alignment mode
		frontal_alignment.distance_mode = 1; // NORMAL distance mode
		//lateral_alignment.alignment_mode = 9; // RIGHT_LASER alignment mode
		//lateral_alignment.distance_mode = 1; // NORMAL distance mode
		align_client_.sendGoal(frontal_alignment);
		state_ = store_puck_states::ALIGNING_FRONTAL;
		align_client_.waitForResult();

		ROS_INFO("Store_number_: %s", StoreStoreNumbers::toString(store_number_).c_str());
		switch(store_number_)
		{
			case store_store_numbers::NEAR:
				{
				while(laser_front_ > 0.13)
					{
						setVelocity(0.15, 0, 0);
						publishVelocity();
					}
				//setVelocity(0.0, 0.0, 0.0);
				//publishVelocity();
				}
				break;
			case store_store_numbers::MIDDLE:
				{
					printf("entrou no MIDDLE");
					while(laser_front_ > 0.78)
					{
						setVelocity(0.15, 0, 0);
						publishVelocity();
					}
				//setVelocity(0.0, 0.0, 0.0);
				//publishVelocity();
				}
				break;
			case store_store_numbers::FAR_AWAY:
				{
					printf("entrou no FAR_AWAY");
					while(laser_front_ > 1.43)
					{
						setVelocity(0.15, 0, 0);
						publishVelocity();
					}
					//setVelocity(0.0, 0.0, 0.0);
					//publishVelocity();
				}
				break;
			case store_store_numbers::VOLTAR_PRA_CASA:
				{
					frontal_alignment.alignment_mode = 8; // FRONT_LASER alignment mode
					frontal_alignment.distance_mode = 1; // NORMAL distance mode
					align_client_.sendGoal(frontal_alignment);
					state_ = store_puck_states::ALIGNING_FRONTAL;
					align_client_.waitForResult();

					while(laser_front_ > 0.13)
					{
						setVelocity(0.15, 0, 0);
						publishVelocity();
					}
					while(laser_left_ > 0.3)
					{
						setVelocity(0, 0.1, 0);
						publishVelocity();
					}
					flag_aux_ = true;
				}
				break;
			default:
				ROS_ERROR("Store Number not supported yet!!!");
		}

		//laserDistanceFront(store_number_);

		if(flag_aux_ == false)
		{
			frontal_alignment.alignment_mode = 8; // FRONT_LASER alignment mode
			frontal_alignment.distance_mode = 1; // NORMAL distance mode
			align_client_.sendGoal(frontal_alignment);
			state_ = store_puck_states::ALIGNING_FRONTAL;
			align_client_.waitForResult();

			resetOdometry();
			while(getOdometry_PHI() < PI / 2)
			{
				setVelocity(0, 0, 0.5);
				publishVelocity();
			}
			frontal_alignment.alignment_mode = 8; // FRONT_LASER alignment mode
			frontal_alignment.distance_mode = 1; // NORMAL distance mode
			align_client_.sendGoal(frontal_alignment);
			state_ = store_puck_states::ALIGNING_FRONTAL;
			align_client_.waitForResult();

			state_ = store_puck_states::STORING_PUCK;
			while(laser_front_ > 0.33)
			{
				setVelocity(0.15, 0, 0);
				publishVelocity();
			}
			state_ = store_puck_states::LEAVING_PUCK;
			while(laser_front_ < 0.8)
			{
				setVelocity(-0.2, 0, 0);
				publishVelocity();
			}
			if(laser_right_ < 0.6 || laser_left_ < 0.6)
			{
				while(laser_right_ < 0.6)
				{
					setVelocity(0, 0.2, 0);
					publishVelocity();
				}
				while(laser_left_ < 0.6)
				{
					setVelocity(0, -0.2, 0);
					publishVelocity();
				}
			}
		}
	}
	else if (mode_ == store_modes::VISION)
	{
		if (state_ != store_puck_states::LEAVING_PUCK && state_ != store_puck_states::GOING_BACK_TO_ORIGIN)
		{
			robotino_vision::FindInsulatingTapeAreas srv;
			find_areas_cli_.waitForExistence();
			if (!find_areas_cli_.call(srv))
			{	
				ROS_ERROR("Area not found!!!");
				state_ = store_puck_states::LOST;
				return;
			}
			std::vector<float> distances = srv.response.distances;
			std::vector<float> directions = srv.response.directions; 
			int num_areas = srv.response.distances.size();
			if (num_areas > 0 && state_ != store_puck_states::STORING_PUCK)
			{
				int closest_area_index = 0;
				for (int i = 0; i < num_areas; i++)
				{
					if (distances[i] < distances[closest_area_index] && fabs(directions[i]) <fabs(directions[closest_area_index]))
					{
						closest_area_index = i;
					}
				}
				double max_error_lateral = 50, max_error_frontal = 40;
				double error_lateral = directions[closest_area_index];
				if (error_lateral > max_error_lateral)
				{
					 error_lateral = max_error_lateral;
				}
				double error_frontal = distances[closest_area_index];
				if (error_frontal > max_error_frontal)
				{
					 error_frontal = max_error_frontal;
				}
				float tolerance_lateral = 0.1, tolerance_frontal = 30;
				double K_error_lateral = 0.3, K_error_frontal = 0.003;
				double percentage_f, percentage_0, tolerance, max_error, error;
				if (fabs(error_lateral) > tolerance_lateral) // 0% a 49%
				{
					state_ = store_puck_states::ALIGNING_LATERAL;
					vel_y = -K_error_lateral * error_lateral;
					percentage_0 = 0;
					percentage_f = 49;
					tolerance = tolerance_lateral;
					max_error = max_error_lateral;
					error = fabs(error_lateral);
				}
				else if (fabs(error_frontal) > tolerance_frontal) // 50% a 69%
				{
					state_ = store_puck_states::HEADING_TOWARD_AREA;
					vel_x = K_error_frontal * error_frontal;
					percentage_0 = 50;
					percentage_f = 69;
					tolerance = tolerance_frontal;
					max_error = max_error_frontal;
					error = fabs(error_frontal);
				}
				else 
				{
					vel_x = .14;
					state_ = store_puck_states::STORING_PUCK;
					storing_start_ = ros::Time::now();
				}
				percentage = percentage_0 + (percentage_f - percentage_0) * (max_error - error) / (max_error - tolerance);
			}
			else if (state_ == store_puck_states::STORING_PUCK) // 70% a 89%
			{
				vel_x = .1;
				double percentage_0 = 70, percentage_f = 89;
				double elapsed_time = (ros::Time::now() - storing_start_).toSec();
				if (elapsed_time > STORING_DEADLINE)
				{
					state_ = store_puck_states::LEAVING_PUCK;
				}
				percentage = percentage_0 + (percentage_f - percentage_0) * elapsed_time / STORING_DEADLINE;
			}
		}
		else
		{
			if (state_ == store_puck_states::LEAVING_PUCK)
			{
				delta_x_ = -getOdometry_X();
				vel_x = -.14;
				resetOdometry();
				state_ = store_puck_states::GOING_BACK_TO_ORIGIN;
				percentage_ = 89;
			}
			double max_error_linear = 1.25;
			double error_linear = delta_x_ -  getOdometry_X();
			if (error_linear > max_error_linear)
			{
				 error_linear = max_error_linear;
			}
			float tolerance_linear = 0.01;
			double K_error_linear = 1.2;
			double percentage_f, percentage_0, tolerance, max_error, error;
			if (fabs(error_linear) > tolerance_linear) // 90% a 99%
			{
				state_ = store_puck_states::GOING_BACK_TO_ORIGIN;
				vel_x = K_error_linear * error_linear;
				percentage_0 = 91;
				percentage_f = 99;
				tolerance = tolerance_linear;
				max_error = max_error_linear;
				error = fabs(error_linear);			
			}
			percentage = percentage_0 + (percentage_f - percentage_0) * (max_error - error) / (max_error - tolerance);
		}
	}
	else
	{
		ROS_ERROR("Storage Mode not supported yet!!!");
		return;
	}
	if (vel_x == 0 && vel_y == 0 && vel_phi == 0) // 100%
	{
		state_ = store_puck_states::FINISHED;
		percentage_ = 100;
	}
	if (percentage > percentage_)
	{
		percentage_ = percentage;
	}
	setVelocity(vel_x, vel_y, vel_phi);
	publishVelocity();
	publishFeedback();
}
void AprilMessageReceived(const apriltags::AprilTagDetections& detectionsMsg) {

    	double tmp_roll, tmp_pitch, tmp_yaw;
	//AR frame: AprilTags Frame
	//Q-frame: Quadrotor Frame
	//if an AR tag is detected then get the positions, if not, then don't, so you don't segfault
	if (&detectionsMsg.detections[0] != NULL){
		
        	
		CurrentPoseStamped.header = detectionsMsg.header;
		//pose vector of quadrotor relative to AR tags as detected by the April Tags package
		//These position coordinates have an error that needs to be corrected for, because the 
		//quadrotor yaw coordinates have an error relative to the April Tag fixed coordinate system
		//so need to correct for that error
		CurrentPoseStamped.pose.position.x = -detectionsMsg.detections[0].pose.position.x;
		CurrentPoseStamped.pose.position.y = detectionsMsg.detections[0].pose.position.y;
		CurrentPoseStamped.pose.position.z = detectionsMsg.detections[0].pose.position.z;

		CurrentPoseStamped.pose.orientation.x = detectionsMsg.detections[0].pose.orientation.x;
		CurrentPoseStamped.pose.orientation.y = detectionsMsg.detections[0].pose.orientation.y;
		CurrentPoseStamped.pose.orientation.z = detectionsMsg.detections[0].pose.orientation.z;
		CurrentPoseStamped.pose.orientation.w = detectionsMsg.detections[0].pose.orientation.w;

		tf::Quaternion quat(CurrentPoseStamped.pose.orientation.x, CurrentPoseStamped.pose.orientation.y, CurrentPoseStamped.pose.orientation.z, CurrentPoseStamped.pose.orientation.w);
		tf::Matrix3x3 m(quat);
		m.getRPY(tmp_roll, tmp_pitch, tmp_yaw); //values from quadrotor in quadrotor original coordinates (need to be corrected for)
	
		//**Converting ARTag coordinate position to Q-frame using yaw error correction
		curr_Q_X = cos(curr_Q_Yaw)*CurrentPoseStamped.pose.position.x  + sin(curr_Q_Yaw)*CurrentPoseStamped.pose.position.y + offset_X;
		curr_Q_Y = -sin(curr_Q_Yaw)*CurrentPoseStamped.pose.position.x + cos(curr_Q_Yaw)*CurrentPoseStamped.pose.position.y + offset_Y;
		curr_Q_Z = CurrentPoseStamped.pose.position.z + offset_Z;
	
		//**Calculate yaw between AR frame and Q-frame from quaternions in detectionsMsg
		//getting scalar of Yaw and converting from quarternion into ENU coordinate frame	
		curr_Q_Yaw = -tmp_yaw;

		ROS_INFO("AprilTags Detected");
        }else {
		ROS_ERROR("No AprilTags Detected");
	}	  

	//**Calculate velocity from current and previous positions and delta_time_between_calls

	// Time since last call
        double delta_time_between_calls = (ros::Time::now() - prev_time).toSec();
        prev_time = ros::Time::now();

        // Calculate velocity
        if (delta_time_between_calls < 1.0){
		prev_vel_X = (prev_Q_X - curr_Q_X)/delta_time_between_calls;
		prev_vel_Y = (prev_Q_Y - curr_Q_Y)/delta_time_between_calls;
		prev_vel_Z = (prev_Q_Z - curr_Q_Z)/delta_time_between_calls;
        } else {
		prev_vel_X = 0.0;
		prev_vel_Y = 0.0;
		prev_vel_Z = 0.0;
        }
	//print info 	
	/*char tab2[1024];
        strncpy(tab2, mode.c_str(), sizeof(tab2));
        tab2[sizeof(tab2) - 1] = 0;
        ROS_INFO("Quadrotor_Curr_Coordinates = (%f , %f) | Previous_Coordinates = (%f , %f) \n delta_time_between_calls = %fs | prev_vel_X = (%f , %f)\n Roll = %f | Pitch = %f\n Mode = %s \n", curr_Q_X, curr_Q_Y, prev_Q_X, prev_Q_Y, delta_time_between_calls, prev_vel_X, prev_vel_Y, Roll, Pitch, tab2);
	*/
	 // Error between Quadrotor coordinates and April Tag coordinates in ENU (East North Up = X Y Z)
	float ErX = 0.0;
	float ErY = 0.0;
	float ErZ = 0.0;
	float ErYaw = 0.0;
	
        prev_Q_X = curr_Q_X;
        prev_Q_Y = curr_Q_Y;
	prev_Q_Z = curr_Q_Z;	

	// Calculate the error between Quadrotor center and ARTag center
	ErX = des_X - curr_Q_X;
	ErY = des_Y - curr_Q_Y;
	ErZ = des_Z - curr_Q_Z;
	ErYaw = des_Yaw - curr_Q_Yaw; 

        // Calculate Roll, Pitch, Throttle, Yaw depending on the mode
	 if (mode == "ALT_HOLD"){
		Roll = BASERC + FACTOR_ROLL*(0.5*ErX+0.1*prev_vel_X);
		Pitch = BASERC + FACTOR_PITCH*(0.5*ErY+0.1*prev_vel_Y); 
		Throttle = BASERC + FACTOR_THROTTLE*(0.5*ErZ+0.1*prev_vel_Z);
		Yaw = BASERC + FACTOR_YAW*(0.1*ErYaw);
        } else{
		Roll = BASERC;
		Pitch = BASERC;
		Throttle = BASERC;
		Yaw = BASERC;
        }  
         
        // Limit the Roll
        if (Roll > MAXRC){
            Roll = MAXRC;
        } else if (Roll < MINRC){
            Roll = MINRC;
        }

        // Limit the Pitch
        if (Pitch > MAXRC){
            Pitch = MAXRC;
        } else if (Pitch < MINRC){
            Pitch = MINRC;
        }	
	
	  // Limit the Throttle
        if (Throttle > MAXRC){
            Throttle = MAXRC;
        } else if (Throttle < MINRC){
            Throttle = MINRC;
        }
	  // Limit the Yaw
        if (Yaw > MAXRC){
            Yaw = MAXRC;
        } else if (Yaw < MINRC){
            Yaw = MINRC;
        }	

	//Publish new values to msg.channels[] as shown below for quadrotor flight controller
        msg.channels[0] = Roll;     //Roll
        msg.channels[1] = Pitch;    //Pitch
        msg.channels[2] = Throttle;   //Throttle
        msg.channels[3] = Yaw;        //Yaw
        msg.channels[4] = 0;
        msg.channels[5] = 0;
        msg.channels[6] = 0;
        msg.channels[7] = 0;

	ROS_INFO("ROLL: %0.1f	PITCH: %0.1f	THROTTLE: %0.1f	   YAW: %0.1f", Roll, Pitch, Throttle, Yaw);

        pub.publish(msg); 
}