Ejemplo n.º 1
0
void JointTrajectoryStreamer::streamingThread()
{
  int connectRetryCount = 1;

  ROS_INFO("Starting joint trajectory streamer thread");
  while (ros::ok())
  {
    ros::Duration(0.005).sleep();

    // automatically re-establish connection, if required
    if (connectRetryCount-- > 0)
    {
      ROS_INFO("Connecting to robot motion server");
      this->connection_->makeConnect();
      ros::Duration(0.250).sleep();  // wait for connection

      if (this->connection_->isConnected())
        connectRetryCount = 0;
      else if (connectRetryCount <= 0)
      {
        ROS_ERROR("Timeout connecting to robot controller.  Send new motion command to retry.");
        this->state_ = TransferStates::IDLE;
      }
      continue;
    }

    this->mutex_.lock();

    SimpleMessage msg, tmpMsg, reply;
        
    switch (this->state_)
    {
      case TransferStates::IDLE:
        ros::Duration(0.250).sleep();  //  slower loop while waiting for new trajectory
        break;

      case TransferStates::STREAMING:
        if (this->current_point_ >= (int)this->current_traj_.size())
        {
          ROS_INFO("Trajectory streaming complete, setting state to IDLE");
          this->state_ = TransferStates::IDLE;
          break;
        }

        if (!this->connection_->isConnected())
        {
          ROS_DEBUG("Robot disconnected.  Attempting reconnect...");
          connectRetryCount = 5;
          break;
        }

        tmpMsg = this->current_traj_[this->current_point_];
        msg.init(tmpMsg.getMessageType(), CommTypes::SERVICE_REQUEST,
                 ReplyTypes::INVALID, tmpMsg.getData());  // set commType=REQUEST
            
        ROS_DEBUG("Sending joint trajectory point");
        if (this->connection_->sendAndReceiveMsg(msg, reply, false))
        {
          ROS_INFO("Point[%d of %d] sent to controller",
                   this->current_point_, (int)this->current_traj_.size());
          this->current_point_++;
        }
        else
          ROS_WARN("Failed sent joint point, will try again");

        break;
      default:
        ROS_ERROR("Joint trajectory streamer: unknown state");
        this->state_ = TransferStates::IDLE;
        break;
    }

    this->mutex_.unlock();
  }

  ROS_WARN("Exiting trajectory streamer thread");
}
  virtual bool adaptAndPlan(const PlannerFn &planner,
                            const planning_scene::PlanningSceneConstPtr& planning_scene,
                            const planning_interface::MotionPlanRequest &req,
                            planning_interface::MotionPlanResponse &res,
                            std::vector<std::size_t> &added_path_index) const
  {
    ROS_DEBUG("Running '%s'", getDescription().c_str());

    // get the specified start state
    robot_state::RobotState start_state = planning_scene->getCurrentState();
    robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state);

    // if the start state is otherwise valid but does not meet path constraints
    if (planning_scene->isStateValid(start_state, req.group_name) &&
        !planning_scene->isStateValid(start_state, req.path_constraints, req.group_name))
    {
      ROS_INFO("Path constraints not satisfied for start state...");
      planning_scene->isStateValid(start_state, req.path_constraints, req.group_name, true);
      ROS_INFO("Planning to path constraints...");

      planning_interface::MotionPlanRequest req2 = req;
      req2.goal_constraints.resize(1);
      req2.goal_constraints[0] = req.path_constraints;
      req2.path_constraints = moveit_msgs::Constraints();
      planning_interface::MotionPlanResponse res2;
      // we call the planner for this additional request, but we do not want to include potential 
      // index information from that call
      std::vector<std::size_t> added_path_index_temp;
      added_path_index_temp.swap(added_path_index);
      bool solved1 = planner(planning_scene, req2, res2);
      added_path_index_temp.swap(added_path_index);
      
      if (solved1)
      {
        planning_interface::MotionPlanRequest req3 = req;
        ROS_INFO("Planned to path constraints. Resuming original planning request.");

        // extract the last state of the computed motion plan and set it as the new start state
        robot_state::robotStateToRobotStateMsg(res2.trajectory_->getLastWayPoint(), req3.start_state);
        bool solved2 = planner(planning_scene, req3, res);
        res.planning_time_ += res2.planning_time_;

        if (solved2)
        {
          // since we add a prefix, we need to correct any existing index positions
          for (std::size_t i = 0 ; i < added_path_index.size() ; ++i)
            added_path_index[i] += res2.trajectory_->getWayPointCount();
          
          // we mark the fact we insert a prefix path (we specify the index position we just added)
          for (std::size_t i = 0 ; i < res2.trajectory_->getWayPointCount() ; ++i)
            added_path_index.push_back(i);
          
          // we need to append the solution paths.
          res2.trajectory_->append(*res.trajectory_, 0.0);
          res2.trajectory_->swap(*res.trajectory_);
          return true;
        }
        else
          return false;
      }
      else
      {
        ROS_WARN("Unable to plan to path constraints. Running usual motion plan.");
        bool result = planner(planning_scene, req, res);
        res.planning_time_ += res2.planning_time_;
        return result;
      }
    }
    else
    {
      ROS_DEBUG("Path constraints are OK. Running usual motion plan.");
      return planner(planning_scene, req, res);
    }
  }
 void 
   noFilterCallback (const sensor_msgs::PointCloud2ConstPtr &cloud)
 {
   pointCloudPublisher_.publish (cloud);
   ROS_DEBUG ("Self filter publishing unfiltered frame");
 }
Ejemplo n.º 4
0
void set_speed_callback(const drive::speed::ConstPtr& msg)
{
	driveChain.setMotorSpeeds((int)(msg->speedLeft * 1000000.0), (int)(msg->speedRight * 1000000.0));
  ROS_DEBUG("set_speed_callback(): setting speeds %.2f / %.2f according to message on topic", msg->speedLeft, msg->speedRight);
}
Ejemplo n.º 5
0
  /* 
   * One and only one callback, now takes cloud, does everything else needed. 
   */
  void ARPublisher::getTransformationCallback (const sensor_msgs::PointCloud2ConstPtr & msg)
  {
    sensor_msgs::ImagePtr image_msg(new sensor_msgs::Image);
    ARUint8 *dataPtr;
    ARMarkerInfo *marker_info;
    int marker_num;
    int i, k, j;

    /* do we need to initialize? */
    if(!configured_)
    {
      if(msg->width == 0 || msg->height == 0)
      {
        ROS_ERROR ("Deformed cloud! Size = %d, %d.", msg->width, msg->height);
        return;
      }

      cam_param_.xsize = msg->width;
      cam_param_.ysize = msg->height;

      cam_param_.dist_factor[0] = msg->width/2;         // x0 = cX from openCV calibration
      cam_param_.dist_factor[1] = msg->height/2;        // y0 = cY from openCV calibration
      cam_param_.dist_factor[2] = 0;                    // f = -100*k1 from CV. Note, we had to do mm^2 to m^2, hence 10^8->10^2
      cam_param_.dist_factor[3] = 1.0;                  // scale factor, should probably be >1, but who cares...
      
      arInit ();
    }

    /* convert cloud to PCL */
    PointCloud cloud;
    pcl::fromROSMsg(*msg, cloud);
 
    /* get an OpenCV image from the cloud */
    pcl::toROSMsg (cloud, *image_msg);
    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 ());
    }
    dataPtr = (ARUint8 *) capture_->imageData;

    /* detect the markers in the video frame */
    if (arDetectMarkerLite (dataPtr, threshold_, &marker_info, &marker_num) < 0)
    {
      argCleanup ();
      return;
    }
 
    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;
      }
      
      /* create a cloud for marker corners */
      int d = marker_info[k].dir;
      PointCloud marker;
      marker.push_back( cloud.at( (int)marker_info[k].vertex[(4-d)%4][0], (int)marker_info[k].vertex[(4-d)%4][1] ) ); // upper left
      marker.push_back( cloud.at( (int)marker_info[k].vertex[(5-d)%4][0], (int)marker_info[k].vertex[(5-d)%4][1] ) ); // upper right
      marker.push_back( cloud.at( (int)marker_info[k].vertex[(6-d)%4][0], (int)marker_info[k].vertex[(6-d)%4][1] ) ); // lower right
      marker.push_back( cloud.at( (int)marker_info[k].vertex[(7-d)%4][0], (int)marker_info[k].vertex[(7-d)%4][1] ) );

      /* create an ideal cloud */
      double w = object[i].marker_width;
      PointCloud ideal;
      ideal.push_back( makeRGBPoint(-w/2,w/2,0) );
      ideal.push_back( makeRGBPoint(w/2,w/2,0) );
      ideal.push_back( makeRGBPoint(w/2,-w/2,0) );
      ideal.push_back( makeRGBPoint(-w/2,-w/2,0) );

      /* get transformation */
      Eigen::Matrix4f t;
      pcl::estimateRigidTransformationSVD( marker, ideal, t );
      
      /* get final transformation */
      tf::Transform transform = tfFromEigen(t.inverse());
   
      // any(transform == nan)
      btMatrix3x3  m = transform.getBasis();
      btVector3    p = transform.getOrigin();
      bool invalid = false;
      for(int i=0; i < 3; i++)
        for(int j=0; j < 3; j++)
          invalid = (invalid || isnan(m[i][j]) || fabs(m[i][j]) > 1.0);

      for(int i=0; i < 3; i++)
          invalid = (invalid || isnan(p[i]));
       

      if(invalid)
        continue; 

      /* publish the marker */
      ar_pose::ARMarker ar_pose_marker;
      ar_pose_marker.header.frame_id = msg->header.frame_id;
      ar_pose_marker.header.stamp = msg->header.stamp;
      ar_pose_marker.id = object[i].id;

      ar_pose_marker.pose.pose.position.x = transform.getOrigin().getX();
      ar_pose_marker.pose.pose.position.y = transform.getOrigin().getY();
      ar_pose_marker.pose.pose.position.z = transform.getOrigin().getZ();

      ar_pose_marker.pose.pose.orientation.x = transform.getRotation().getAxis().getX();
      ar_pose_marker.pose.pose.orientation.y = transform.getRotation().getAxis().getY();
      ar_pose_marker.pose.pose.orientation.z = transform.getRotation().getAxis().getZ();
      ar_pose_marker.pose.pose.orientation.w = transform.getRotation().getW();

      ar_pose_marker.confidence = marker_info->cf;
      arPoseMarkers_.markers.push_back (ar_pose_marker);

      /* publish transform */
      if (publishTf_)
      {
	    broadcaster_.sendTransform(tf::StampedTransform(transform, msg->header.stamp, msg->header.frame_id, object[i].name));
      }

      /* publish visual marker */

      if (publishVisualMarkers_)
      {
        btVector3 markerOrigin (0, 0, 0.25 * object[i].marker_width * AR_TO_ROS);
        btTransform m (btQuaternion::getIdentity (), markerOrigin);
        btTransform markerPose = transform * m; // marker pose in the camera frame

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

        rvizMarker_.header.frame_id = msg->header.frame_id;
        rvizMarker_.header.stamp = 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 ();

        rvizMarkerPub_.publish (rvizMarker_);
        ROS_DEBUG ("Published visual marker");
      }
    }
    arMarkerPub_.publish (arPoseMarkers_);
    ROS_DEBUG ("Published ar_multi markers");
  }
Ejemplo n.º 6
0
void EncoderOdometryNode::publishEstimatedOdomMsg(
const ros::TimerEvent& timer_event) {
    // Check to make sure that we have some counts for both the left and right
    // encoders
    if (!left_encoder_num_ticks_curr || !right_encoder_num_ticks_curr) {
        // If we don't have either the left or right encoder ticks, just return
        return;
    }

    // Checks if we've made an estimate before
    if (!left_encoder_num_ticks_prev || !right_encoder_num_ticks_prev) {
        ROS_DEBUG("Got first ticks from encoders");
        // If we haven't, save the current state as the previous state
        // and just return. The first actual estimate will be made at the
        // next call to this function
        left_encoder_num_ticks_prev  = left_encoder_num_ticks_curr;
        right_encoder_num_ticks_prev = right_encoder_num_ticks_curr;

        // Save the time so that next time we get ticks, we can
        // generate a state estimate
        last_estimate.header.stamp = ros::Time::now();

        return;
    }

    // All math taken from:
    // https://www.cs.cmu.edu/afs/cs.cmu.edu/academic/class/16311/www/s07/labs/NXTLabs/Lab%203.html

    double dt = ros::Time::now().toSec() - last_estimate.header.stamp.toSec();

    // left and right wheel velocities (in meters/s)
    int left_encoder_ticks =
    *left_encoder_num_ticks_curr - *left_encoder_num_ticks_prev;
    int right_encoder_ticks =
    *right_encoder_num_ticks_curr - *right_encoder_num_ticks_prev;
    double v_l =
    (left_encoder_ticks) / dt * (2 * M_PI) / ticks_per_rotation * wheel_radius;
    double v_r =
    (right_encoder_ticks) / dt * (2 * M_PI) / ticks_per_rotation * wheel_radius;

    // linear and angular velocity
    double v = (v_r + v_l) / 2;
    double w = (v_r - v_l) / wheelbase_length;

    // x and y velocity
    double x_vel = v * cos(w);
    double y_vel = v * sin(w);

    // position
    double prev_x   = last_estimate.pose.pose.position.x;
    double prev_y   = last_estimate.pose.pose.position.y;
    double prev_yaw = tf::getYaw(last_estimate.pose.pose.orientation);

    double k_00 = v * cos(prev_yaw);
    double k_01 = v * sin(prev_yaw);
    double k_02 = w;
    double k_10 = v * cos(prev_yaw + dt / 2 * k_02);
    double k_11 = v * sin(prev_yaw + dt / 2 * k_02);
    double k_12 = w;
    double k_20 = v * cos(prev_yaw + dt / 2 * k_12);
    double k_21 = v * sin(prev_yaw + dt / 2 * k_12);
    double k_22 = w;
    double k_30 = v * cos(prev_yaw + dt * k_22);
    double k_31 = v * sin(prev_yaw + dt * k_22);
    double k_32 = w;

    double x   = prev_x + (dt / 6) * (k_00 + 2 * (k_10 + k_20) + k_30);
    double y   = prev_y + (dt / 6) * (k_01 + 2 * (k_11 + k_21) + k_31);
    double yaw = prev_yaw + (dt / 6) * (k_02 + 2 * (k_12 + k_22) + k_32);

    // Update our estimate
    last_estimate.pose.pose.position.x = x;
    last_estimate.pose.pose.position.y = y;
    tf::quaternionTFToMsg(tf::createQuaternionFromYaw(yaw),
                          last_estimate.pose.pose.orientation);
    last_estimate.twist.twist.linear.x  = x_vel;
    last_estimate.twist.twist.linear.y  = y_vel;
    last_estimate.twist.twist.angular.z = w;
    last_estimate.header.stamp          = ros::Time::now();
    last_estimate.header.frame_id       = odom_frame_id;

    // Save the current number of ticks as the "previous" number for the
    // next time we estimate
    left_encoder_num_ticks_prev  = left_encoder_num_ticks_curr;
    right_encoder_num_ticks_prev = right_encoder_num_ticks_curr;

    // Publish our estimate
    odom_estimate_publisher.publish(last_estimate);
}
Ejemplo n.º 7
0
long long
hokuyo::Laser::calcLatency(bool intensity, double min_ang, double max_ang, int clustering, int skip, int num, int timeout)
{
  ROS_DEBUG("Entering calcLatency.");

  if (!portOpen())
    HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");

  offset_ = 0;

  uint64_t comp_time = 0;
  uint64_t laser_time = 0;
  long long diff_time = 0;
  long long drift_time = 0;
  long long tmp_offset1 = 0;
  long long tmp_offset2 = 0;

  int count = 0;
 
  sendCmd("TM0",timeout);
  count = 100;

  for (int i = 0; i < count;i++)
  {
    usleep(1000);
    sendCmd("TM1",timeout);
    comp_time = timeHelper();
    try 
    {
      laser_time = readTime();

      diff_time = comp_time - laser_time;

      tmp_offset1 += diff_time / count;
    } catch (hokuyo::RepeatedTimeException &e)
    {
      // We expect to get Repeated Time's when hammering on the time server
      continue;
    }
  }

  uint64_t start_time = timeHelper();
  usleep(5000000);
  sendCmd("TM1;a",timeout);
  sendCmd("TM1;b",timeout);
  comp_time = timeHelper();
  drift_time = comp_time - start_time;
  laser_time = readTime() + tmp_offset1;
  diff_time = comp_time - laser_time;
  double drift_rate = double(diff_time) / double(drift_time);

  sendCmd("TM2",timeout);
  
  if (requestScans(intensity, min_ang, max_ang, clustering, skip, num, timeout) != 0)
    HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting scans during latency calculation");

  hokuyo::LaserScan scan;

  count = 200;
  for (int i = 0; i < count;i++)
  {
    try
    {
      serviceScan(scan, 1000);
    } catch (hokuyo::CorruptedDataException &e) {
      continue;
    }

    comp_time = scan.system_time_stamp;
    drift_time = comp_time - start_time;
    laser_time = scan.self_time_stamp + tmp_offset1 + (long long)(drift_time*drift_rate);
    diff_time = laser_time - comp_time;

    tmp_offset2 += diff_time / count;
  }

  offset_ = tmp_offset2;

  stopScanning();
  
  ROS_DEBUG("Leaving calcLatency.");

  return offset_;
}
Ejemplo n.º 8
0
// returns true, iff node could be added to the cloud
bool GraphManager::addNode(Node new_node) {


    std::clock_t starttime=std::clock();
    if(reset_request_) {
        int numLevels = 3;
        int nodeDistance = 2;
        marker_id =0;
        time_of_last_transform_= ros::Time();
        last_batch_update_=std::clock();
        delete optimizer_;
        optimizer_ = new AIS::HCholOptimizer3D(numLevels, nodeDistance);
        graph_.clear();
        matches_.clear();
        freshlyOptimized_= false;
        reset_request_ = false;
    }


    if (new_node.feature_locations_2d_.size() <= 5) {
        ROS_DEBUG("found only %i features on image, node is not included",(int)new_node.feature_locations_2d_.size());
        return false;
    }

    //set the node id only if the node is actually added to the graph
    //needs to be done here as the graph size can change inside this function
    new_node.id_ = graph_.size();

    //First Node, so only build its index, insert into storage and add a
    //vertex at the origin, of which the position is very certain
    if (graph_.size()==0) {
        new_node.buildFlannIndex(); // create index so that next nodes can use it

//        graph_.insert(make_pair(new_node.id_, new_node)); //store
        graph_[new_node.id_] = new_node;
        optimizer_->addVertex(0, Transformation3(), 1e9*Matrix6::eye(1.0)); //fix at origin
        return true;
    }


    unsigned int num_edges_before = optimizer_->edges().size();


    std::vector<cv::DMatch> initial_matches;
    ROS_DEBUG("Graphsize: %d", (int) graph_.size());
    marker_id = 0; //overdraw old markers


    Eigen::Matrix4f ransac_trafo, final_trafo;

    bool edge_added_to_base;
    std::vector<int> vertices_to_comp = getPotentialEdgeTargets(new_node, -1); //vernetzungsgrad
    int id_of_id = vertices_to_comp.size() -1;
    for (; id_of_id >=0; id_of_id--) { //go from the back, so the last comparison is with the first node. The last comparison is visualized.
        initial_matches = processNodePair(new_node, graph_[vertices_to_comp[id_of_id]],edge_added_to_base,ransac_trafo, final_trafo);
        //initial_matches = processNodePair(new_node, graph_rit->second);
        //What if the node has not been added? visualizeFeatureFlow3D(graph_rit->second, new_node, initial_matches, matches_, marker_id++);
    }
    id_of_id++;//set back to last valid id

    if (optimizer_->edges().size() > num_edges_before) {

        new_node.buildFlannIndex();
        graph_[new_node.id_] = new_node;

        ROS_DEBUG("Added Node, new Graphsize: %i", (int) graph_.size());

        optimizeGraph();
        //make the transform of the last node known
        ros::TimerEvent unused;
        broadcastTransform(unused);
        visualizeGraphEdges();
        visualizeGraphNodes();
        visualizeFeatureFlow3D(graph_[vertices_to_comp[id_of_id]], new_node, initial_matches, matches_, marker_id++);
    } else {
        ROS_WARN("##### could not find link for PointCloud!");
        matches_.clear();
    }
    ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "function runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC  <<"sec");
    return (optimizer_->edges().size() > num_edges_before);
}
Ejemplo n.º 9
0
void GraphManager::visualizeFeatureFlow3D(const Node& earlier_node,
        const Node& newer_node,
        const std::vector<cv::DMatch>& all_matches,
        const std::vector<cv::DMatch>& inlier_matches,
        unsigned int marker_id,
        bool draw_outlier) const {
    std::clock_t starttime=std::clock();
    if (ransac_marker_pub_.getNumSubscribers() > 0) { //don't visualize, if nobody's looking

        visualization_msgs::Marker marker_lines;

        marker_lines.header.frame_id = "/openni_rgb_optical_frame";
        marker_lines.ns = "ransac_markers";
        marker_lines.header.stamp = ros::Time::now();
        marker_lines.action = visualization_msgs::Marker::ADD;
        marker_lines.pose.orientation.w = 1.0;
        marker_lines.id = marker_id;
        marker_lines.type = visualization_msgs::Marker::LINE_LIST;
        marker_lines.scale.x = 0.002;

        std_msgs::ColorRGBA color_red  ;  //red outlier
        color_red.r = 1.0;
        color_red.a = 1.0;
        std_msgs::ColorRGBA color_green;  //green inlier, newer endpoint
        color_green.g = 1.0;
        color_green.a = 1.0;
        std_msgs::ColorRGBA color_yellow;  //yellow inlier, earlier endpoint
        color_yellow.r = 1.0;
        color_yellow.g = 1.0;
        color_yellow.a = 1.0;
        std_msgs::ColorRGBA color_blue  ;  //red-blue outlier
        color_blue.b = 1.0;
        color_blue.a = 1.0;

        marker_lines.color = color_green; //just to set the alpha channel to non-zero

        AISNavigation::PoseGraph3D::Vertex* earlier_v; //used to get the transform
        AISNavigation::PoseGraph3D::Vertex* newer_v; //used to get the transform
        AISNavigation::PoseGraph3D::VertexIDMap v_idmap = optimizer_->vertices();
        // end of initialization
        ROS_DEBUG("Matches Visualization start");

        // write all inital matches to the line_list
        marker_lines.points.clear();//necessary?

        if (draw_outlier)
        {
            for (unsigned int i=0; i<all_matches.size(); i++) {
                int newer_id = all_matches.at(i).queryIdx; //feature id in newer node
                int earlier_id = all_matches.at(i).trainIdx; //feature id in earlier node

                earlier_v = static_cast<AISNavigation::PoseGraph3D::Vertex*>(v_idmap[earlier_node.id_]);
                newer_v = static_cast<AISNavigation::PoseGraph3D::Vertex*>(v_idmap[newer_node.id_]);

                //Outliers are red (newer) to blue (older)
                marker_lines.colors.push_back(color_red);
                marker_lines.colors.push_back(color_blue);

                marker_lines.points.push_back(
                    pointInWorldFrame(newer_node.feature_locations_3d_[newer_id],
                                      newer_v->transformation));
                marker_lines.points.push_back(
                    pointInWorldFrame(earlier_node.feature_locations_3d_[earlier_id],
                                      earlier_v->transformation));
            }
        }


        for (unsigned int i=0; i<inlier_matches.size(); i++) {
            int newer_id = inlier_matches.at(i).queryIdx; //feature id in newer node
            int earlier_id = inlier_matches.at(i).trainIdx; //feature id in earlier node

            earlier_v = static_cast<AISNavigation::PoseGraph3D::Vertex*>(v_idmap[earlier_node.id_]);
            newer_v = static_cast<AISNavigation::PoseGraph3D::Vertex*>(v_idmap[newer_node.id_]);


            //inliers are green (newer) to blue (older)
            marker_lines.colors.push_back(color_green);
            marker_lines.colors.push_back(color_blue);

            marker_lines.points.push_back(
                pointInWorldFrame(newer_node.feature_locations_3d_[newer_id],
                                  newer_v->transformation));
            marker_lines.points.push_back(
                pointInWorldFrame(earlier_node.feature_locations_3d_[earlier_id],
                                  earlier_v->transformation));
        }



        ransac_marker_pub_.publish(marker_lines);
        ROS_DEBUG_STREAM("Published  " << marker_lines.points.size()/2 << " lines");
    }
    ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "function runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC  <<"sec");
}
Ejemplo n.º 10
0
bool ControllerManager::unloadController(const std::string &name)
{
  ROS_DEBUG("Will unload controller '%s'", name.c_str());

  // lock the controllers
  boost::recursive_mutex::scoped_lock guard(controllers_lock_);

  // get reference to controller list
  int free_controllers_list = (current_controllers_list_ + 1) % 2;
  while (ros::ok() && free_controllers_list == used_by_realtime_){
    if (!ros::ok())
      return false;
    usleep(200);
  }
  std::vector<ControllerSpec>
    &from = controllers_lists_[current_controllers_list_],
    &to = controllers_lists_[free_controllers_list];
  to.clear();

  // Transfers the running controllers over, skipping the one to be removed and the running ones.
  bool removed = false;
  for (size_t i = 0; i < from.size(); ++i)
  {
    if (from[i].info.name == name){
      if (from[i].c->isRunning()){
        to.clear();
        ROS_ERROR("Could not unload controller with name %s because it is still running",
                  name.c_str());
        return false;
      }
      removed = true;
    }
    else
      to.push_back(from[i]);
  }

  // Fails if we could not remove the controllers
  if (!removed)
  {
    to.clear();
    ROS_ERROR("Could not unload controller with name %s because no controller with this name exists",
              name.c_str());
    return false;
  }

  // Destroys the old controllers list when the realtime thread is finished with it.
  ROS_DEBUG("Realtime switches over to new controller list");
  int former_current_controllers_list_ = current_controllers_list_;
  current_controllers_list_ = free_controllers_list;
  while (ros::ok() && used_by_realtime_ == former_current_controllers_list_){
    if (!ros::ok())
      return false;
    usleep(200);
  }
  ROS_DEBUG("Destruct controller");
  from.clear();
  ROS_DEBUG("Destruct controller finished");

  ROS_DEBUG("Successfully unloaded controller '%s'", name.c_str());
  return true;
}
Ejemplo n.º 11
0
bool ControllerManager::switchController(const std::vector<std::string>& start_controllers,
                                         const std::vector<std::string>& stop_controllers,
                                         int strictness)
{
  if (!stop_request_.empty() || !start_request_.empty())
    ROS_FATAL("The switch controller stop and start list are not empty that the beginning of the swithcontroller call. This should not happen.");

  if (strictness == 0){
    ROS_WARN("Controller Manager: To switch controllers you need to specify a strictness level of controller_manager_msgs::SwitchController::STRICT (%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT.",
             controller_manager_msgs::SwitchController::Request::STRICT,
             controller_manager_msgs::SwitchController::Request::BEST_EFFORT);
    strictness = controller_manager_msgs::SwitchController::Request::BEST_EFFORT;
  }

  ROS_DEBUG("switching controllers:");
  for (unsigned int i=0; i<start_controllers.size(); i++)
    ROS_DEBUG(" - starting controller %s", start_controllers[i].c_str());
  for (unsigned int i=0; i<stop_controllers.size(); i++)
    ROS_DEBUG(" - stopping controller %s", stop_controllers[i].c_str());

  // lock controllers
  boost::recursive_mutex::scoped_lock guard(controllers_lock_);

  controller_interface::ControllerBase* ct;
  // list all controllers to stop
  for (unsigned int i=0; i<stop_controllers.size(); i++)
  {
    ct = getControllerByName(stop_controllers[i]);
    if (ct == NULL){
      if (strictness ==  controller_manager_msgs::SwitchController::Request::STRICT){
        ROS_ERROR("Could not stop controller with name %s because no controller with this name exists",
                  stop_controllers[i].c_str());
        stop_request_.clear();
        return false;
      }
      else{
        ROS_DEBUG("Could not stop controller with name %s because no controller with this name exists",
                  stop_controllers[i].c_str());
      }
    }
    else{
      ROS_DEBUG("Found controller %s that needs to be stopped in list of controllers",
                stop_controllers[i].c_str());
      stop_request_.push_back(ct);
    }
  }
  ROS_DEBUG("Stop request vector has size %i", (int)stop_request_.size());

  // list all controllers to start
  for (unsigned int i=0; i<start_controllers.size(); i++)
  {
    ct = getControllerByName(start_controllers[i]);
    if (ct == NULL){
      if (strictness ==  controller_manager_msgs::SwitchController::Request::STRICT){
        ROS_ERROR("Could not start controller with name %s because no controller with this name exists",
                  start_controllers[i].c_str());
        stop_request_.clear();
        start_request_.clear();
        return false;
      }
      else{
        ROS_DEBUG("Could not start controller with name %s because no controller with this name exists",
                  start_controllers[i].c_str());
      }
    }
    else{
      ROS_DEBUG("Found controller %s that needs to be started in list of controllers",
                start_controllers[i].c_str());
      start_request_.push_back(ct);
    }
  }
  ROS_DEBUG("Start request vector has size %i", (int)start_request_.size());

  // Do the resource management checking
  std::list<hardware_interface::ControllerInfo> info_list;
  std::vector<ControllerSpec> &controllers = controllers_lists_[current_controllers_list_];
  for (size_t i = 0; i < controllers.size(); ++i)
  {
    bool in_stop_list  = false;

    for(size_t j = 0; j < stop_request_.size(); j++)
      in_stop_list = in_stop_list || (stop_request_[j] == controllers[i].c.get());

    bool in_start_list = false;
    for(size_t j = 0; j < start_request_.size(); j++)
      in_start_list = in_start_list || (start_request_[j] == controllers[i].c.get());

    bool add_to_list = controllers[i].c->isRunning();
    if (in_stop_list)
      add_to_list = false;
    if (in_start_list)
      add_to_list = true;

    if (add_to_list)
      info_list.push_back(controllers[i].info);
  }

  bool in_conflict = robot_hw_->checkForConflict(info_list);
  if (in_conflict)
  {
    ROS_ERROR("Could not switch controllers, due to resource conflict");
    stop_request_.clear();
    start_request_.clear();
    return false;
  }

  // start the atomic controller switching
  switch_strictness_ = strictness;
  please_switch_ = true;

  // wait until switch is finished
  ROS_DEBUG("Request atomic controller switch from realtime loop");
  while (ros::ok() && please_switch_){
    if (!ros::ok())
      return false;
    usleep(100);
  }
  start_request_.clear();
  stop_request_.clear();

  ROS_DEBUG("Successfully switched controllers");
  return true;
}
Ejemplo n.º 12
0
bool ControllerManager::loadController(const std::string& name)
{
  ROS_DEBUG("Will load controller '%s'", name.c_str());

  // lock controllers
  boost::recursive_mutex::scoped_lock guard(controllers_lock_);

  // get reference to controller list
  int free_controllers_list = (current_controllers_list_ + 1) % 2;
  while (ros::ok() && free_controllers_list == used_by_realtime_){
    if (!ros::ok())
      return false;
    usleep(200);
  }
  std::vector<ControllerSpec>
    &from = controllers_lists_[current_controllers_list_],
    &to = controllers_lists_[free_controllers_list];
  to.clear();

  // Copy all controllers from the 'from' list to the 'to' list
  for (size_t i = 0; i < from.size(); ++i)
    to.push_back(from[i]);

  // Checks that we're not duplicating controllers
  for (size_t j = 0; j < to.size(); ++j)
  {
    if (to[j].info.name == name)
    {
      to.clear();
      ROS_ERROR("A controller named '%s' was already loaded inside the controller manager", name.c_str());
      return false;
    }
  }

  ros::NodeHandle c_nh;
  // Constructs the controller
  try{
    c_nh = ros::NodeHandle(root_nh_, name);
  }
  catch(std::exception &e) {
    ROS_ERROR("Exception thrown while constructing nodehandle for controller with name '%s':\n%s", name.c_str(), e.what());
    return false;
  }
  catch(...){
    ROS_ERROR("Exception thrown while constructing nodehandle for controller with name '%s'", name.c_str());
    return false;
  }
  boost::shared_ptr<controller_interface::ControllerBase> c;
  std::string type;
  if (c_nh.getParam("type", type))
  {
    ROS_DEBUG("Constructing controller '%s' of type '%s'", name.c_str(), type.c_str());
    try
    {
      // Trying loading the controller using all of our controller loaders. Exit once we've found the first valid loaded controller
      std::list<LoaderPtr>::iterator it = controller_loaders_.begin();
      while (!c && it != controller_loaders_.end())
      {
        std::vector<std::string> cur_types = (*it)->getDeclaredClasses();
        for(size_t i=0; i < cur_types.size(); i++){
          if (type == cur_types[i]){
            c = (*it)->createInstance(type);
          }
        }
        ++it;
      }
    }
    catch (const std::runtime_error &ex)
    {
      ROS_ERROR("Could not load class %s: %s", type.c_str(), ex.what());
    }
  }
  else
  {
    ROS_ERROR("Could not load controller '%s' because the type was not specified. Did you load the controller configuration on the parameter server?", name.c_str());
    to.clear();
    return false;
  }

  // checks if controller was constructed
  if (!c)
  {
    ROS_ERROR("Could not load controller '%s' because controller type '%s' does not exist.",  name.c_str(), type.c_str());
    ROS_ERROR("Use 'rosservice call controller_manager/list_controller_types' to get the available types");
    to.clear();
    return false;
  }

  // Initializes the controller
  ROS_DEBUG("Initializing controller '%s'", name.c_str());
  bool initialized;
  std::set<std::string> claimed_resources; // Gets populated during initRequest call
  try{
    initialized = c->initRequest(robot_hw_, root_nh_, c_nh, claimed_resources);
  }
  catch(std::exception &e){
    ROS_ERROR("Exception thrown while initializing controller %s.\n%s", name.c_str(), e.what());
    initialized = false;
  }
  catch(...){
    ROS_ERROR("Exception thrown while initializing controller %s", name.c_str());
    initialized = false;
  }
  if (!initialized)
  {
    to.clear();
    ROS_ERROR("Initializing controller '%s' failed", name.c_str());
    return false;
  }
  ROS_DEBUG("Initialized controller '%s' succesful", name.c_str());

  // Adds the controller to the new list
  to.resize(to.size() + 1);
  to[to.size()-1].info.type = type;
  to[to.size()-1].info.hardware_interface = c->getHardwareInterfaceType();
  to[to.size()-1].info.name = name;
  to[to.size()-1].info.resources = claimed_resources;
  to[to.size()-1].c = c;

  // Destroys the old controllers list when the realtime thread is finished with it.
  int former_current_controllers_list_ = current_controllers_list_;
  current_controllers_list_ = free_controllers_list;
  while (ros::ok() && used_by_realtime_ == former_current_controllers_list_){
    if (!ros::ok())
      return false;
    usleep(200);
  }
  from.clear();

  ROS_DEBUG("Successfully load controller '%s'", name.c_str());
  return true;
}
/// \brief callback for setting models joints states
bool setModelsJointsStates(pr2_gazebo_plugins::SetModelsJointsStates::Request &req,
                           pr2_gazebo_plugins::SetModelsJointsStates::Response &res)
{
  ROS_DEBUG("setModelsJointsStates");
  return true;
}
void GazeboRosControllerManager::ReadPr2Xml()
{

  std::string urdf_param_name;
  std::string urdf_string;
  // search and wait for robot_description on param server
  while(urdf_string.empty())
  {
    ROS_DEBUG("gazebo controller manager plugin is waiting for urdf: %s on the param server.", this->robotParam.c_str());
    if (this->rosnode_->searchParam(this->robotParam,urdf_param_name))
    {
      this->rosnode_->getParam(urdf_param_name,urdf_string);
      ROS_DEBUG("found upstream\n%s\n------\n%s\n------\n%s",this->robotParam.c_str(),urdf_param_name.c_str(),urdf_string.c_str());
    }
    else
    {
      this->rosnode_->getParam(this->robotParam,urdf_string);
      ROS_DEBUG("found in node namespace\n%s\n------\n%s\n------\n%s",this->robotParam.c_str(),urdf_param_name.c_str(),urdf_string.c_str());
    }
    usleep(100000);
  }
  ROS_DEBUG("gazebo controller manager got pr2.xml from param server, parsing it...");

  // 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());
  }
  else
  {
    //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->hw_.addActuator(pr2_actuator);
    }

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

    for (unsigned int i = 0; i < this->cm_->state_->joint_states_.size(); ++i)
      this->cm_->state_->joint_states_[i].calibrated_ = fake_calibration_;
  }
}
void DecosSensorRing::ReadSensorRingValues()
{
    int r = 1;

    if(fd == P_ERROR)
    {
        ROS_ERROR("Port not opened. Abort!");
        return;
    }

    int totalrx = readData(fd, rxbuf, 64);
    rxbuf[totalrx] = 0;

    //ROS_INFO("RX: <%s>", rxbuf);

    for (int i=0; i<totalrx; i++)
    {
        unsigned char z = rxbuf[i];

                                // one frame = [I|%03i|%03i|%03i|%03i|%03i|%03i|U|%04i]
        switch (rxstate)        // this is our state machine to read one full frame of values
        {
            case 0: 
                if (z=='[') rxstate++;
                break;
            
            case 1: 
                if (z=='I') 
                    rxstate++;
                else
                    rxstate = 0;
                break;
            
            case 2:
            case 4:
            case 6:
            case 8:
            case 10:
            case 12:
            case 14:
            case 16:
                if (z=='|') 
                {
                    rxstate++;
                    rxcount = 0;
                }
                else
                    rxstate = 0;
                break;
            
            case 3:
            case 5:
            case 7:
            case 9:
            case 11:
            case 13: 
                if (CollectValue(z, 3)==0)  // done with one value
                {
                    int idx = (int)(rxstate-3)/2;
                    if ((idx>=0) && (idx<6))
                        sensorSet.IR[idx] = atoi(collectedvalue);
                }            
                break;
                        
            case 15:
                if (z=='U') 
                    rxstate++;
                else
                    rxstate = 0;
                break;

            case 17: 
                if (CollectValue(z, 4)==0)  // got value?
                {
                    sensorSet.US = atoi(collectedvalue);
                    sensorSet.counter++;
                    sensorSet.valid = true;
                    rxstate = 0;
                }            
                break;

            case 18:    // in case a fail occured at step 17 in CollectValue
                rxstate = 0;
                break;                

        }
    }

    if (sensorSet.valid)
    {
        ROS_DEBUG("Set: %03i, %03i, %03i, %03i, %03i, %03i | %04i", sensorSet.IR[0], sensorSet.IR[1], sensorSet.IR[2], sensorSet.IR[3], sensorSet.IR[4], sensorSet.IR[5], sensorSet.US);

        sensorSet.valid = false;

        decos_sensorring::SensorValues sv;
        sv.IR1 = sensorSet.IR[0];
        sv.IR2 = sensorSet.IR[1];
        sv.IR3 = sensorSet.IR[2];
        sv.IR4 = sensorSet.IR[3];
        sv.IR5 = sensorSet.IR[4];
        sv.IR6 = sensorSet.IR[5];
        sv.US = sensorSet.US;

        sensorPub.publish(sv);
    }
}
//returns a single point cloud colored objects ,
void CObjectCandidateExtraction::extractObjectCandidates(pcl::PointCloud<
		pcl::PointXYZRGB> &point_cloud,
		pcl::PointCloud<pcl::PointXYZRGBNormal> &planar_point_cloud,
		std::vector<structPlanarSurface> &hierarchyPlanes) {

	ROS_DEBUG("[extractObjectCandidates] extractObjectCandidates started ...");
	ros::Time start, start2, finish, finish2, start3, finish3;
	start = ros::Time::now();
	std::vector<pcl::PointCloud<pcl::PointXYZRGBNormal> > clusteredObjects;
	std::vector<pcl::PointCloud<pcl::PointXYZRGBNormal> > clusteredPlanes;
	pcl::PointCloud<pcl::PointXYZRGBNormal> point_cloud_RGB;
	pcl::PointIndices inliers, inliersObjects;
	pcl::PointCloud<pcl::PointXYZRGBNormal> total_point_cloud,
			point_cloud_normal;
	//std::vector<structPlanarSurface> hierarchyPlanes;
	//	std::vector<bool> delete_total_point_cloud;


	ROS_DEBUG("[extractObjectCandidates] MovingLeastSquares started ... ");
	//	total_point_cloud = point_cloud_normal = this->toolBox.movingLeastSquares(
	//			point_cloud, 0.02f); //0.02f works good with 0.008 subsampling//0.01 veryfast but not good
	total_point_cloud = point_cloud_normal = this->toolBox.movingLeastSquares(
			point_cloud, 0.01f); //0.02f works good //0.01 veryfast but not good
	ROS_DEBUG("[extractObjectCandidates] MovingLeastSquares done ... ");

	hierarchyPlanes = horizontalSurfaceExtractor.extractMultiplePlanes(
			point_cloud_normal, planar_point_cloud, clusteredPlanes, 2);

	if (hierarchyPlanes.empty()) {
		return;
	}

	for (unsigned int indexMaxPointsClusteredPlane = 0; indexMaxPointsClusteredPlane
			< hierarchyPlanes.size(); indexMaxPointsClusteredPlane++) {
		//planar_point_cloud = hierarchyPlanes[indexMaxPointsClusteredPlane].pointCloud;
		//---------------------------------------------
		// get plane hull
		start2 = ros::Time::now();

		pcl::PointCloud<pcl::PointXYZRGBNormal> cloud_hull;
		//	pcl::ConvexHull2D<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> chull;
		//	chull.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZRGBNormal> > (planar_point_cloud));
		//	chull.reconstruct (cloud_hull);

		cloud_hull = hierarchyPlanes[indexMaxPointsClusteredPlane].convexHull; //pointCloud;

		//ROS_DEBUG("[%s/extractObjectCandidates] dZmax %f dZmin %f",this->nodeName.c_str(),dZmax,dZmin );
		pcl::PointCloud<pcl::PointXYZRGBNormal> cloud_objects;
		cloud_objects.header = total_point_cloud.header;

		bool reject = false;
		unsigned int total_point_cloud_size = total_point_cloud.points.size();

//		int chunk = total_point_cloud_size / 4;
		omp_set_num_threads(4);
#pragma omp parallel shared (total_point_cloud,chunk) private(j)
		{
#pragma omp for schedule(dynamic,chunk) nowait
			for (unsigned int j = 0; j < total_point_cloud_size; ++j) {
				//here check also whether point below upper plane is
				if (toolBox.pointInsideConvexHull2d(cloud_hull,
						total_point_cloud.points[j])
						&& total_point_cloud.points[j].z
								> (toolBox.getNearestNeighborPlane(
										hierarchyPlanes[indexMaxPointsClusteredPlane],
										total_point_cloud.points[j]).z + this->threshold_point_above_lower_plane))
				//(dZmax		+ this->threshold_point_above_lower_plane))//(dZmax+this->threshold_point_above_lower_plane)) //dZmax //dZmax-(dZmax*0.005)) //(((dZmax+dZmin)/2)+dZmax)/2)
				{
					reject = false;
					for (unsigned int iterUpperPlanes = 0; iterUpperPlanes
							< hierarchyPlanes[indexMaxPointsClusteredPlane].upperPlanarSurfaces.size(); iterUpperPlanes++) {
						/*
						if (toolBox.pointInsideConvexHull2d(
								hierarchyPlanes[indexMaxPointsClusteredPlane].upperPlanarSurfaces[iterUpperPlanes].convexHull,
								total_point_cloud.points[j])
								&& total_point_cloud.points[j].z
										> hierarchyPlanes[indexMaxPointsClusteredPlane].upperPlanarSurfaces[iterUpperPlanes].plane_height
												+ THRESHOLD_POINT_ABOVE_UPPER_PLANE) //a little bit over surface(upperplane) in order to get a cube as a cube and not without uppersurface
						{
							reject = true;
							break;
						}*/
					}

					if (!reject) {
						cloud_objects.points.push_back(
								total_point_cloud.points[j]);
					}
				}
			}
		} //PRAGMA

		cloud_objects.width = cloud_objects.points.size();
		cloud_objects.height = 1;
		//---------------------------------------------

		ROS_DEBUG ("[extractObjectCandidates] Number of object point candidates: %d.", (int)cloud_objects.points.size());

		if ((unsigned int) cloud_objects.points.size() > 0) {
			//Cluster objects
			pcl::EuclideanClusterExtraction<pcl::PointXYZRGBNormal>
					euclideanClusterExtractor;
			std::vector<pcl::PointIndices> clusteredObjectIndices;
			euclideanClusterExtractor.setInputCloud(cloud_objects.makeShared());
			euclideanClusterExtractor.setClusterTolerance(0.025); //(0.02);
			euclideanClusterExtractor.setMinClusterSize(30);
			euclideanClusterExtractor.extract(clusteredObjectIndices);

			ROS_DEBUG ("[extractObjectCandidates] Number of objects clustered: %d",(int)clusteredObjectIndices.size());

			///clusteredObjects.resize((int)clusteredObjectIndices.size());

			for (unsigned int iterCluster = 0; iterCluster
					< clusteredObjectIndices.size(); iterCluster++) {
				pcl::PointCloud<pcl::PointXYZRGBNormal> foundObject;
				pcl::PointCloud<pcl::PointXYZRGBNormal> foundObjectFull;
				pcl::PointCloud<pcl::PointXYZRGBNormal> foundObjectHull;
				//pcl::copyPointCloud(cloud_objects,clusteredObjectIndices.at(iterCluster),clusteredObjects.at(iterCluster) );
				pcl::copyPointCloud(cloud_objects, clusteredObjectIndices.at(
						iterCluster), foundObject);
				if (!toolBox.isObjectPlane(
						hierarchyPlanes[indexMaxPointsClusteredPlane],
						foundObject, IS_PLANE_OBJECT__OBJECT_HEIGHT_THRESHOLD,
						IS_PLANE_OBJECT__OBJECT_PLANE_HEIGHT_DIFFERENCE) && foundObject.points.size()>this->min_points_per_objects) {
					ROS_DEBUG("[extractObjectCandidates] Object(%d) added",iterCluster);
					/*
					 pcl::ConvexHull2D<pcl::PointXYZRGBNormal,
					 pcl::PointXYZRGBNormal> convexHullExtractor;
					 convexHullExtractor.setInputCloud(boost::make_shared<
					 pcl::PointCloud<pcl::PointXYZRGBNormal> >(
					 foundObject));
					 convexHullExtractor.reconstruct(foundObjectHull);

					 toolBox.get3DPointsWithinHull(total_point_cloud, foundObjectHull,
					 0, toolBox.maxValuePointCloud3d(foundObject, 2)-toolBox.minValuePointCloud3d(foundObject, 2),
					 foundObjectFull);

					 */
					foundObjectFull = foundObject;
					hierarchyPlanes[indexMaxPointsClusteredPlane].clusteredObjects.push_back(
							foundObjectFull);
				}
			}

			//-------------------------------------------------------------------------------------------------
			//add potential related points to objects (below the object and plane), how it is only interesting if we use it for rgb classfication

			//pcl::KdTreeANN<pcl::PointXYZRGB>::Ptr objectTree = boost::make_shared<pcl::KdTreeANN<pcl::PointXYZRGB> > ();
			//objectTree->setInputCloud(total_point_cloud);
			/*
			 for(unsigned int j=0; j < clusteredObjects.size(); ++j)
			 {
			 double dXObjectmin = 90, dYObjectmin = 90, dXObjectmax = -90, dYObjectmax=-90, dZObjectmax=-90, dZObjectmin=90;
			 double dObjectHeight;

			 for(unsigned int i=0; i < clusteredObjects[j].points.size(); ++i)
			 {
			 if(clusteredObjects[j].points[i].x < dXObjectmin)
			 dXObjectmin = clusteredObjects[j].points[i].x;
			 if(clusteredObjects[j].points[i].x > dXObjectmax)
			 dXObjectmax = clusteredObjects[j].points[i].x;

			 if(clusteredObjects[j].points[i].y < dYObjectmin)
			 dYObjectmin = clusteredObjects[j].points[i].y;
			 if(clusteredObjects[j].points[i].y > dYObjectmax)
			 dYObjectmax = clusteredObjects[j].points[i].y;

			 if(clusteredObjects[j].points[i].z < dZObjectmin)
			 dZObjectmin = clusteredObjects[j].points[i].z;
			 if(clusteredObjects[j].points[i].z > dZObjectmax)
			 dZObjectmax = clusteredObjects[j].points[i].z;
			 }

			 dObjectHeight = dZObjectmax - dZObjectmin;

			 ROS_INFO ("objects no %d -> points %d", j, (int)clusteredObjects[j].points.size());




			 bool found=false;
			 for(unsigned int k=0; k < total_point_cloud.points.size(); ++k)
			 {
			 if(total_point_cloud.points[k].x >= dXObjectmin && total_point_cloud.points[k].x <= dXObjectmax &&
			 total_point_cloud.points[k].y >= dYObjectmin && total_point_cloud.points[k].y <= dYObjectmax &&
			 total_point_cloud.points[k].z >= (dZmax)) //dZmin
			 {
			 //check if point already in point cloud
			 for(unsigned int l=0;l < clusteredObjects[j].points.size(); ++l )
			 {
			 if(clusteredObjects[j].points[l].x==total_point_cloud.points[k].x &&
			 clusteredObjects[j].points[l].y==total_point_cloud.points[k].y &&
			 clusteredObjects[j].points[l].z==total_point_cloud.points[k].z)
			 {
			 found=true;
			 break;
			 }
			 }

			 if(found==false)
			 clusteredObjects[j].points.push_back(total_point_cloud.points[k]);
			 found=false;
			 }
			 }
			 ROS_INFO ("objects no %d -> points %d", j, (int)clusteredObjects[j].points.size());
			 }
			 */
			//------------------

			//TIP:pcl->delete tabletop-> get each object pcl-> do distance intensity -> perfect object training without any texture (project object on black background so we can apply 2d/3d stuff)
			//   : then take several view, do the same, do ICP/toro or merge all points to get a 3d model for viewpoint estimation e.g.
			//   : match points from query with icp to find best fit. but we need to know before hand what kind of object we found-> so apply recognition/classification beforehand
			////
		}
		finish2 = ros::Time::now();
		ROS_DEBUG("[extractObjectCandidates] %d. plane computed - (%lf)",indexMaxPointsClusteredPlane ,(finish2.toSec() - start2.toSec() ));
	}
	//clusteredObjectsInput = clusteredObjects;
	finish = ros::Time::now();
	ROS_DEBUG("[extractObjectCandidates] Execution time = %lf",(finish.toSec() - start.toSec() ));
}
Ejemplo n.º 17
0
  /** Dynamic reconfigure callback
   *
   *  Called immediately when callback first defined. Called again
   *  when dynamic reconfigure starts or changes a parameter value.
   *
   *  @param newconfig new Config values
   *  @param level bit-wise OR of reconfiguration levels for all
   *               changed parameters (0xffffffff on initial call)
   **/
  void Camera1394Driver::reconfig(Config &newconfig, uint32_t level)
  {
    // Do not run concurrently with poll().  Tell it to stop running,
    // and wait on the lock until it does.
    reconfiguring_ = true;
    boost::mutex::scoped_lock lock(mutex_);
    ROS_DEBUG("dynamic reconfigure level 0x%x", level);

    // resolve frame ID using tf_prefix parameter
    if (newconfig.frame_id == "")
      newconfig.frame_id = "camera";
    std::string tf_prefix = tf::getPrefixParam(priv_nh_);
    ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
    newconfig.frame_id = tf::resolve(tf_prefix, newconfig.frame_id);

    if (state_ != Driver::CLOSED && (level & Levels::RECONFIGURE_CLOSE))
      {
        // must close the device before updating these parameters
        closeCamera();                  // state_ --> CLOSED
      }

    if (state_ == Driver::CLOSED)
      {
        // open with new values
        openCamera(newconfig);
      }

    if (config_.camera_info_url != newconfig.camera_info_url)
      {
        if (!validateConfig(newconfig))
          {
            // new URL not valid, use the old one
            newconfig.camera_info_url = config_.camera_info_url;
          }
      }

    if (state_ != Driver::CLOSED)       // openCamera() succeeded?
      {
        // configure IIDC features
        if (level & Levels::RECONFIGURE_CLOSE)
          {
            // initialize all features for newly opened device
            if (false == dev_->features_->initialize(&newconfig))
              {
                ROS_ERROR_STREAM("[" << camera_name_
                                 << "] feature initialization failure");
                closeCamera();          // can't continue
              }
          }
        else
          {
            // update any features that changed
            // TODO replace this with a dev_->reconfigure(&newconfig);
            dev_->features_->reconfigure(&newconfig);
          }
      }

    config_ = newconfig;                // save new parameters
    reconfiguring_ = false;             // let poll() run again

    ROS_DEBUG_STREAM("[" << camera_name_
                     << "] reconfigured: frame_id " << newconfig.frame_id
                     << ", camera_info_url " << newconfig.camera_info_url);
  }
void Commander::keyLoop() {
	char c;

	// get the console in raw mode                                                              
	tcgetattr(kfd, &cooked);
	memcpy(&raw, &cooked, sizeof(struct termios));
	raw.c_lflag &=~ (ICANON | ECHO);
	// Setting a new line, then end of file                         
	raw.c_cc[VEOL] = 1;
	raw.c_cc[VEOF] = 2;
	tcsetattr(kfd, TCSANOW, &raw);

	puts("Reading from keyboard");
	puts("---------------------------");
	puts("Use arrow keys to move the ROBOT.");


	for(;;) {
		// get the next event from the keyboard  
		LunarNXT::Order msg;
  	
		if(read(kfd, &c, 1) < 0) {
			perror("read():");
			exit(-1);
		}

		ROS_DEBUG("value: 0x%02X\n", c);
  
		switch(c) {
			case KEYCODE_Q:
				ROS_INFO("Turn Left !");
				msg.sOrder = "turn_l";
			        break;
			case KEYCODE_D:
				ROS_INFO("Turn Right !");
				msg.sOrder = "turn_r";
				break;
			case KEYCODE_Z:
				ROS_INFO("Go !");
				msg.sOrder = "go";
			        break;
			case KEYCODE_S:
				ROS_INFO("Back !");
				msg.sOrder = "back";
				break;
			case KEYCODE_A:
				ROS_INFO("Stop !");
				msg.sOrder = "stop";
				break;
			case KEYCODE_L:
				ROS_INFO("LineLauncher");
				msg.sOrder = "launch";
				break;
			case KEYCODE_M:
				ROS_INFO("LineUnLauncher");
				msg.sOrder = "unlaunch";
				break;
		}

		this->commander_pub.publish(msg);
	}
	return;
}
bool GridMapRosConverter::addLayerFromImage(const sensor_msgs::Image& image,
                                            const std::string& layer, grid_map::GridMap& gridMap,
                                            const double lowerValue, const double upperValue)
{
  cv_bridge::CvImagePtr cvPtrAlpha, cvPtrMono;

  // If alpha channel exist, read it.
  if (image.encoding == sensor_msgs::image_encodings::BGRA8
      || image.encoding == sensor_msgs::image_encodings::BGRA16) {
    try {
      cvPtrAlpha = cv_bridge::toCvCopy(image, image.encoding);
    } catch (cv_bridge::Exception& e) {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return false;
    }
  }

  unsigned int depth;
  // Convert color image to grayscale.
  try {
    if (image.encoding == sensor_msgs::image_encodings::BGRA8
        || image.encoding == sensor_msgs::image_encodings::BGR8
        || image.encoding == sensor_msgs::image_encodings::MONO8) {
      cvPtrMono = cv_bridge::toCvCopy(image,
                                      sensor_msgs::image_encodings::MONO8);
      depth = std::pow(2, 8);
      ROS_DEBUG("Color image converted to mono8");
    } else if (image.encoding == sensor_msgs::image_encodings::BGRA16
        || image.encoding == sensor_msgs::image_encodings::BGR16
        || image.encoding == sensor_msgs::image_encodings::MONO16) {
      cvPtrMono = cv_bridge::toCvCopy(image,
                                      sensor_msgs::image_encodings::MONO16);
      depth = std::pow(2, 16);
      ROS_DEBUG("Color image converted to mono16");
    } else {
      ROS_ERROR("Expected BGR, BGRA, or MONO image encoding.");
      return false;
    }
  } catch (cv_bridge::Exception& e) {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return false;
  }

  gridMap.add(layer);

  if (gridMap.getSize()(0) != image.height
      || gridMap.getSize()(1) != image.width) {
    ROS_ERROR("Image size does not correspond to grid map size!");
    return false;
  }

  for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
    // Set transparent values.
    if (image.encoding == sensor_msgs::image_encodings::BGRA8) {
      const auto& cvAlpha = cvPtrAlpha->image.at<cv::Vec4b>((*iterator)(0),
                                                            (*iterator)(1));
      unsigned int alpha = cvAlpha[3];
      if (cvAlpha[3] < depth / 2)
        continue;
    }
    if (image.encoding == sensor_msgs::image_encodings::BGRA16) {
      const auto& cvAlpha = cvPtrAlpha->image.at<cv::Vec<uchar, 8>>(
          (*iterator)(0), (*iterator)(1));
      int alpha = (cvAlpha[6] << 8) + cvAlpha[7];
      if (alpha < depth / 2)
        continue;
    }

    // Compute height.
    unsigned int grayValue;
    if (depth == std::pow(2, 8)) {
      uchar cvGrayscale = cvPtrMono->image.at<uchar>((*iterator)(0),
                                                     (*iterator)(1));
      grayValue = cvGrayscale;
    }
    if (depth == std::pow(2, 16)) {
      const auto& cvGrayscale = cvPtrMono->image.at<cv::Vec2b>((*iterator)(0),
                                                               (*iterator)(1));
      grayValue = (cvGrayscale[0] << 8) + cvGrayscale[1];
    }

    double height = lowerValue
        + (upperValue - lowerValue) * ((double) grayValue / (double) (depth - 1));
    gridMap.at(layer, *iterator) = height;
  }

  return true;
}
void CostMapCalculation::callbackElevationMap(const hector_elevation_msgs::ElevationGridConstPtr& elevation_map_msg)
{
    ROS_DEBUG("HectorCM: received new elevation map");

    // check header
    if((int)(1000*elevation_map_msg->info.resolution_xy) != (int)(1000*cost_map.info.resolution) &&  // Magic number 1000 -> min grid size should not be less than 1mm
            elevation_map_msg->info.height != cost_map.info.height &&
            elevation_map_msg->info.width != cost_map.info.width)
    {
        ROS_ERROR("HectorCM: elevation map resolution and or size incorrect!");
        return;
    }

    // store elevation_map_msg in local variable
    elevation_map_ = cv::Mat(elevation_map_msg->info.height, elevation_map_msg->info.width, CV_16S, const_cast<int16_t*>(&elevation_map_msg->data[0]), 2*(size_t)elevation_map_msg->info.width);

    // store elevation map zero level
    elevation_zero_level = elevation_map_msg->info.zero_elevation;

    // compute region of intereset
    if(!computeWindowIndices(elevation_map_msg->header.stamp, update_radius_world))
        return;

    // loop through each element
    int filtered_cell, filtered_cell_x, filtered_cell_y;
    for (int v = min_index(1); v < max_index(1); ++v)
    {
        for (int u = min_index(0); u < max_index(0); ++u)
        {
            // compute cost_map_index
            unsigned int cost_map_index = MAP_IDX(cost_map.info.width, u, v);

            // check if neighbouring cells are known
            if(elevation_map_.at<int16_t>(v+1,u) == (-elevation_zero_level) ||
                    elevation_map_.at<int16_t>(v-1,u) == (-elevation_zero_level) ||
                    elevation_map_.at<int16_t>(v,u+1) == (-elevation_zero_level) ||
                    elevation_map_.at<int16_t>(v,u-1) == (-elevation_zero_level))
                continue;

            // edge filter
            filtered_cell_y = abs(elevation_map_.at<int16_t>(v,u-1) - elevation_map_.at<int16_t>(v,u+1));
            filtered_cell_x = abs(elevation_map_.at<int16_t>(v-1,u) - elevation_map_.at<int16_t>(v+1,u));


            if(filtered_cell_x > filtered_cell_y)
                filtered_cell = filtered_cell_x;
            else
                filtered_cell =  filtered_cell_y;

            // check if cell is traversable
            if(filtered_cell > max_delta_elevation/grid_res_z)
            {
                // cell is not traversable -> mark it as occupied
                elevation_cost_map.data[cost_map_index] = OCCUPIED_CELL;
            }
            else
            {
                // cell is traversable -> mark it as free
                elevation_cost_map.data[cost_map_index] = FREE_CELL;
            }
        }
    }

    // set elevation map received flag
    received_elevation_map = true;

    // calculate cost map
    if(received_grid_map)
    {
        if(received_point_cloud)
        {
            calculateCostMap(USE_GRID_AND_ELEVATION_MAP_AND_CLOUD_MAP);
        }
        else
        {
            calculateCostMap(USE_GRID_AND_ELEVATION_MAP);
        }
    }
    else
    {
        calculateCostMap(USE_ELEVATION_MAP_ONLY);
    }
}
int HandymanTeleopKey::run(int argc, char **argv)
{
  char c;

  /////////////////////////////////////////////
  // get the console in raw mode
  int kfd = 0;
  struct termios cooked;

  struct termios raw;
  tcgetattr(kfd, &cooked);
  memcpy(&raw, &cooked, sizeof(struct termios));
  raw.c_lflag &=~ (ICANON | ECHO);
  raw.c_cc[VEOL] = 1;
  raw.c_cc[VEOF] = 2;
  tcsetattr(kfd, TCSANOW, &raw);
  /////////////////////////////////////////////

  showHelp();

  ros::init(argc, argv, "handyman_teleop_key");

  ros::NodeHandle node_handle;

  // Override the default ros sigint handler.
  // This must be set after the first NodeHandle is created.
  signal(SIGINT, rosSigintHandler);

  ros::Rate loop_rate(40);

  std::string sub_msg_to_robot_topic_name;
  std::string pub_msg_to_moderator_topic_name;
  std::string sub_joint_state_topic_name;
  std::string pub_base_twist_topic_name;
  std::string pub_arm_trajectory_topic_name;
  std::string pub_gripper_trajectory_topic_name;

  node_handle.param<std::string>("sub_msg_to_robot_topic_name",       sub_msg_to_robot_topic_name,       "/handyman/message/to_robot");
  node_handle.param<std::string>("pub_msg_to_moderator_topic_name",   pub_msg_to_moderator_topic_name,   "/handyman/message/to_moderator");

  node_handle.param<std::string>("sub_joint_state_topic_name",        sub_joint_state_topic_name,        "/hsrb/joint_states");
  node_handle.param<std::string>("pub_base_twist_topic_name",         pub_base_twist_topic_name,         "/hsrb/opt_command_velocity");
  node_handle.param<std::string>("pub_arm_trajectory_topic_name",     pub_arm_trajectory_topic_name,     "/hsrb/arm_trajectory_controller/command");
  node_handle.param<std::string>("pub_gripper_trajectory_topic_name", pub_gripper_trajectory_topic_name, "/hsrb/gripper_trajectory_controller/command");


  ros::Subscriber sub_msg                = node_handle.subscribe<handyman::HandymanMsg>(sub_msg_to_robot_topic_name, 100, &HandymanTeleopKey::messageCallback, this);
  ros::Publisher  pub_msg                = node_handle.advertise<handyman::HandymanMsg>(pub_msg_to_moderator_topic_name, 10);
  ros::Subscriber sub_joint_state        = node_handle.subscribe<sensor_msgs::JointState>(sub_joint_state_topic_name, 10, &HandymanTeleopKey::jointStateCallback, this);
  ros::Publisher  pub_base_twist         = node_handle.advertise<geometry_msgs::Twist>            (pub_base_twist_topic_name, 10);
  ros::Publisher  pub_arm_trajectory     = node_handle.advertise<trajectory_msgs::JointTrajectory>(pub_arm_trajectory_topic_name, 10);
  ros::Publisher  pub_gripper_trajectory = node_handle.advertise<trajectory_msgs::JointTrajectory>(pub_gripper_trajectory_topic_name, 10);


  const float linear_coef         = 0.2f;
  const float linear_oblique_coef = 0.141f;
  const float angular_coef        = 0.5f;

  float move_speed = 1.0f;
  bool is_hand_open = false;

  std::string arm_lift_joint_name   = "arm_lift_joint";
  std::string arm_flex_joint_name   = "arm_flex_joint";
  std::string wrist_flex_joint_name = "wrist_flex_joint";

  while (ros::ok())
  {
    if(canReceive(kfd))
    {
      // get the next event from the keyboard
      if(read(kfd, &c, 1) < 0)
      {
        perror("read():");
        exit(EXIT_FAILURE);
      }

      switch(c)
      {
        case KEYCODE_0:
        {
          sendMessage(pub_msg, MSG_I_AM_READY);
          break;
        }
        case KEYCODE_1:
        {
          sendMessage(pub_msg, MSG_ROOM_REACHED);
          break;
        }
        case KEYCODE_2:
        {
          sendMessage(pub_msg, MSG_OBJECT_GRASPED);
          break;
        }
        case KEYCODE_3:
        {
          sendMessage(pub_msg, MSG_TASK_FINISHED);
          break;
        }
        case KEYCODE_6:
        {
          sendMessage(pub_msg, MSG_DOES_NOT_EXIST);
          break;
        }
        case KEYCODE_9:
        {
          sendMessage(pub_msg, MSG_GIVE_UP);
          break;
        }
        case KEYCODE_UP:
        {
          ROS_DEBUG("Go Forward");
          moveBase(pub_base_twist, +linear_coef*move_speed, 0.0, 0.0);
          break;
        }
        case KEYCODE_DOWN:
        {
          ROS_DEBUG("Go Backward");
          moveBase(pub_base_twist, -linear_coef*move_speed, 0.0, 0.0);
          break;
        }
        case KEYCODE_RIGHT:
        {
          ROS_DEBUG("Go Right");
          moveBase(pub_base_twist, 0.0, 0.0, -angular_coef*move_speed);
          break;
        }
        case KEYCODE_LEFT:
        {
          ROS_DEBUG("Go Left");
          moveBase(pub_base_twist, 0.0, 0.0, +angular_coef*move_speed);
          break;
        }
        case KEYCODE_S:
        {
          ROS_DEBUG("Stop");
          moveBase(pub_base_twist, 0.0, 0.0, 0.0);
          break;
        }
        case KEYCODE_U:
        {
          ROS_DEBUG("Move Left Forward");
          moveBase(pub_base_twist, +linear_oblique_coef*move_speed, +linear_oblique_coef*move_speed, 0.0);
          break;
        }
        case KEYCODE_I:
        {
          ROS_DEBUG("Move Forward");
          moveBase(pub_base_twist, +linear_coef*move_speed, 0.0, 0.0);
          break;
        }
        case KEYCODE_O:
        {
          ROS_DEBUG("Move Right Forward");
          moveBase(pub_base_twist, +linear_oblique_coef*move_speed, -linear_oblique_coef*move_speed, 0.0);
          break;
        }
        case KEYCODE_J:
        {
          ROS_DEBUG("Move Left");
          moveBase(pub_base_twist, 0.0, +linear_coef*move_speed, 0.0);
          break;
        }
        case KEYCODE_K:
        {
          ROS_DEBUG("Stop");
          moveBase(pub_base_twist, 0.0, 0.0, 0.0);
          break;
        }
        case KEYCODE_L:
        {
          ROS_DEBUG("Move Right");
          moveBase(pub_base_twist, 0.0, -linear_coef*move_speed, 0.0);
          break;
        }
        case KEYCODE_M:
        {
          ROS_DEBUG("Move Left Backward");
          moveBase(pub_base_twist, -linear_oblique_coef*move_speed, +linear_oblique_coef*move_speed, 0.0);
          break;
        }
        case KEYCODE_COMMA:
        {
          ROS_DEBUG("Move Backward");
          moveBase(pub_base_twist, -linear_coef*move_speed, 0.0, 0.0);
          break;
        }
        case KEYCODE_PERIOD:
        {
          ROS_DEBUG("Move Right Backward");
          moveBase(pub_base_twist, -linear_oblique_coef*move_speed, -linear_oblique_coef*move_speed, 0.0);
          break;
        }
        case KEYCODE_Q:
        {
          ROS_DEBUG("Move Speed Up");
          move_speed *= 2;
          if(move_speed > 2  ){ move_speed=2; }
          break;
        }
        case KEYCODE_Z:
        {
          ROS_DEBUG("Move Speed Down");
          move_speed /= 2;
          if(move_speed < 0.125){ move_speed=0.125; }
          break;
        }
        case KEYCODE_Y:
        {
          ROS_DEBUG("Move Arm Up");
          moveArm(pub_arm_trajectory, arm_lift_joint_name, 0.69, std::max<int>((int)(std::abs(0.69 - arm_lift_joint_pos1_) / 0.05), 1));
          break;
        }
        case KEYCODE_H:
        {
          ROS_DEBUG("Stop Arm");
          moveArm(pub_arm_trajectory, arm_lift_joint_name, 2.0*arm_lift_joint_pos1_-arm_lift_joint_pos2_, 0.5);
          break;
        }
        case KEYCODE_N:
        {
          ROS_DEBUG("Move Arm Down");
          moveArm(pub_arm_trajectory, arm_lift_joint_name, 0.0, std::max<int>((int)(std::abs(0.0 - arm_lift_joint_pos1_) / 0.05), 1));
          break;
        }
        case KEYCODE_A:
        {
          ROS_DEBUG("Rotate Arm - Vertical");
          moveArm(pub_arm_trajectory, arm_flex_joint_name, 0.0, 1);
          moveArm(pub_arm_trajectory, wrist_flex_joint_name, -1.57, 1);
          break;
        }
        case KEYCODE_B:
        {
          ROS_DEBUG("Rotate Arm - Upward");
          moveArm(pub_arm_trajectory, arm_flex_joint_name, -0.785, 1);
          moveArm(pub_arm_trajectory, wrist_flex_joint_name, -0.785, 1);
          break;
        }
        case KEYCODE_C:
        {
          ROS_DEBUG("Rotate Arm - Horizontal");
          moveArm(pub_arm_trajectory, arm_flex_joint_name, -1.57, 1);
          moveArm(pub_arm_trajectory, wrist_flex_joint_name, 0.0, 1);
          break;
        }
        case KEYCODE_D:
        {
          ROS_DEBUG("Rotate Arm - Downward");
          moveArm(pub_arm_trajectory, arm_flex_joint_name, -2.2, 1);
          moveArm(pub_arm_trajectory, wrist_flex_joint_name, 0.35, 1);
          break;
        }
        case KEYCODE_G:
        {
          moveHand(pub_gripper_trajectory, is_hand_open);

          is_hand_open = !is_hand_open;
          break;
        }
      }
    }

    ros::spinOnce();

    loop_rate.sleep();
  }

  /////////////////////////////////////////////
  // cooked mode
  tcsetattr(kfd, TCSANOW, &cooked);
  /////////////////////////////////////////////

  return EXIT_SUCCESS;
}
void CostMapCalculation::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_sensor (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_base_link (new pcl::PointCloud<pcl::PointXYZ>);

    ROS_DEBUG("HectorCM: received new point cloud");

    // converting PointCloud2 msg format to pcl pointcloud format in order to read the 3d data
    pcl::fromROSMsg(*cloud_msg, *cloud_sensor);

    // transform cloud to /map frame
    try
    {
        listener.waitForTransform("base_stabilized", cloud_msg->header.frame_id,cloud_msg->header.stamp,ros::Duration(1.0));
        pcl_ros::transformPointCloud("base_stabilized",*cloud_sensor,*cloud_base_link,listener);
    }
    catch (tf::TransformException ex)
    {
        ROS_ERROR("%s",ex.what());
        ROS_DEBUG("HectorCM: pointcloud transform failed");
        return;
    }

    // compute region of intereset
    if(!computeWindowIndices(cloud_msg->header.stamp, update_radius_world))
        return;

    Eigen::Vector2f world_coords;
    // define a cube with two points in space:
    Eigen::Vector4f minPoint;
    world_coords = world_map_transform.getC1Coords(min_index.cast<float>());
    minPoint[0]=world_coords(0);  // define minimum point x
    minPoint[1]=world_coords(1);  // define minimum point y
    minPoint[2]=slize_min_height; // define minimum point z

    Eigen::Vector4f maxPoint;
    world_coords = world_map_transform.getC1Coords(max_index.cast<float>());
    maxPoint[0]=world_coords(0);  // define max point x
    maxPoint[1]=world_coords(1);  // define max point y
    maxPoint[2]=slize_max_height; // define max point z

    pcl::CropBox<pcl::PointXYZ> cropFilter;
    cropFilter.setInputCloud (cloud_base_link);
    cropFilter.setMin(minPoint);
    cropFilter.setMax(maxPoint);
    cropFilter.filter (*sliced_cloud);

    ROS_DEBUG("HectorCM: slice size: %d", (int)sliced_cloud->size());
    pub_cloud_slice.publish(sliced_cloud);

    cloud_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);

    // iterate trough all points
    for (unsigned int k = 0; k < sliced_cloud->size(); ++k)
    {
        const pcl::PointXYZ& pt_cloud = sliced_cloud->points[k];

        // allign grid points
        Eigen::Vector2f index_world(pt_cloud.x, pt_cloud.y);
        Eigen::Vector2f index_map (world_map_transform.getC2Coords(index_world));

        cloud_cost_map.data[MAP_IDX(cost_map.info.width, (int)round(index_map(0)), (int)round(index_map(1)))] = OCCUPIED_CELL;
    }

    // set point cloud received flag
    received_point_cloud = true;

    // calculate cost map
    if(received_grid_map)
    {
        if(received_elevation_map)
        {
            calculateCostMap(USE_GRID_AND_ELEVATION_MAP_AND_CLOUD_MAP);
        }
        else
        {
            calculateCostMap(USE_GRID_AND_CLOUD_MAP);
        }
    }
}
Ejemplo n.º 23
0
	/** Dynamic reconfigure callback
	 *
	 *  Called immediately when callback first defined. Called again
	 *  when dynamic reconfigure starts or changes a parameter value.
	 *
	 *  @param newconfig new Config values
	 *  @param level bit-wise OR of reconfiguration levels for all
	 *               changed parameters (0xffffffff on initial call)
	 **/
	void reconfig(Config &newconfig, uint32_t level)
	{
		ROS_DEBUG("dynamic reconfigure level 0x%x", level);

		// resolve frame ID using tf_prefix parameter
		if (newconfig.frame_id == "")
			newconfig.frame_id = "camera";
		std::string tf_prefix = tf::getPrefixParam(privNH_);
		ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
		newconfig.frame_id = tf::resolve(tf_prefix, newconfig.frame_id);

		if (state_ != Driver::CLOSED && (level & Levels::RECONFIGURE_CLOSE))
		{
			// must close the device before updating these parameters
			closeCamera();                  // state_ --> CLOSED
		}

		if (state_ == Driver::CLOSED)
		{
			// open with new values
			if (openCamera(newconfig))
			{
				// update camera name string
				newconfig.camera_name = camera_name_;
			}
		}
		
		
		///THIS IS A QUICK HACK TO GET EXPOSURE ON OUR CAMERA'S THIS DOES NOT WORK FOR ALL CAMERAS

		if(config_.exposure != newconfig.exposure){
			try {
				  cam_->set_control(0x9a0901, newconfig.exposure);
			} catch (uvc_cam::Exception& e) {
				  ROS_ERROR_STREAM("Problem setting exposure. Exception was " << e.what());
			}
		}
		if(config_.absolute_exposure != newconfig.absolute_exposure){
			try {
		  cam_->set_control(0x9a0902, newconfig.absolute_exposure);
		  	} catch (uvc_cam::Exception& e) {
				ROS_ERROR_STREAM("Problem setting absolute exposure. Exception was " << e.what());
			}
		}
		if(config_.sharpness != newconfig.sharpness){
			try {
		  cam_->set_control(0x98091b, newconfig.sharpness);
			} catch (uvc_cam::Exception& e) {
				ROS_ERROR_STREAM("Problem setting sharpness. Exception was " << e.what());
			}
		}
		if(config_.power_line_frequency != newconfig.power_line_frequency){
			try {
		  cam_->set_control(0x980918, newconfig.power_line_frequency);
			} catch (uvc_cam::Exception& e) {
				ROS_ERROR_STREAM("Problem setting powerline frequency. Exception was " << e.what());
			}
		}
		if(config_.white_balance_temperature != newconfig.white_balance_temperature){
			try {
		  cam_->set_control(0x98090c, newconfig.white_balance_temperature);
		  	} catch (uvc_cam::Exception& e) {
				ROS_ERROR_STREAM("Problem setting white balance temperature. Exception was " << e.what());
			}
		}
		if(config_.gain != newconfig.gain){
			try {
		  cam_->set_control(0x980913, newconfig.gain);
		  	} catch (uvc_cam::Exception& e) {
				ROS_ERROR_STREAM("Problem setting gain. Exception was " << e.what());
			}
		}
		if(config_.saturation != newconfig.saturation){
			try {
		  cam_->set_control(0x980902, newconfig.saturation);
		  	} catch (uvc_cam::Exception& e) {
				ROS_ERROR_STREAM("Problem setting saturation. Exception was " << e.what());
			}
		}
		if(config_.contrast != newconfig.contrast){
			try {
		  cam_->set_control(0x980901, newconfig.contrast);
		  	} catch (uvc_cam::Exception& e) {
				ROS_ERROR_STREAM("Problem setting contrast. Exception was " << e.what());
			}
		}
		if(config_.brightness != newconfig.brightness){
			try {
		  cam_->set_control(0x980900, newconfig.brightness);
		  	} catch (uvc_cam::Exception& e) {
				ROS_ERROR_STREAM("Problem setting brightness. Exception was " << e.what());
			}
		}
		
		

		if (config_.camera_info_url != newconfig.camera_info_url)
		{
			// set the new URL and load CameraInfo (if any) from it
			if (cinfo_.validateURL(newconfig.camera_info_url))
			{
				cinfo_.loadCameraInfo(newconfig.camera_info_url);
			}
			else
			{
				// new URL not valid, use the old one
				newconfig.camera_info_url = config_.camera_info_url;
			}
		}

		//	    if (state_ != Driver::CLOSED)       // openCamera() succeeded?
		//	      {
		//	        // configure IIDC features
		//	        if (level & Levels::RECONFIGURE_CLOSE)
		//	          {
		//	            // initialize all features for newly opened device
		//	            if (false == dev_->features_->initialize(&newconfig))
		//	              {
		//	                ROS_ERROR_STREAM("[" << camera_name_
		//	                                 << "] feature initialization failure");
		//	                closeCamera();          // can't continue
		//	              }
		//	          }
		//	        else
		//	          {
		//	            // update any features that changed
		//	            dev_->features_->reconfigure(&newconfig);
		//	          }
		//	      }

		config_ = newconfig;                // save new parameters

		ROS_DEBUG_STREAM("[" << camera_name_
				<< "] reconfigured: frame_id " << newconfig.frame_id
				<< ", camera_info_url " << newconfig.camera_info_url);
	}
bool CostMapCalculation::calculateCostMap_old(char map_level)
{
    switch(map_level)
    {
    case USE_GRID_MAP_ONLY:
    {
        // cost map based on grid map only

        ROS_DEBUG("HectorCM: compute costmap based on grid map");

        // loop through each element
        for (int v = min_index(1); v < max_index(1); ++v)
        {
            for (int u = min_index(0); u < max_index(0); ++u)
            {
                unsigned int index = MAP_IDX(cost_map.info.width, u, v);

                // check if cell is known
                if((char)grid_map_.data[index] != UNKNOWN_CELL)
                {
                    if(grid_map_.data[index] == OCCUPIED_CELL)
                    {
                        // cell is occupied
                        cost_map.data[index] = OCCUPIED_CELL;
                    }
                    else
                    {
                        // cell is not occupied
                        cost_map.data[index] = FREE_CELL;
                    }
                }
            }
        }

        break;
    }
    case USE_ELEVATION_MAP_ONLY:
    {
        // cost map based on elevation map only

        ROS_DEBUG("HectorCM: compute costmap based on elevation map");


        // loop through each element
        for (int v = min_index(1); v < max_index(1); ++v)
        {
            for (int u = min_index(0); u < max_index(0); ++u)
            {
                unsigned int index = MAP_IDX(cost_map.info.width, u, v);

                // check if cell is known
                if(elevation_cost_map.data[index] != UNKNOWN_CELL)
                {
                    if(elevation_cost_map.data[index] == OCCUPIED_CELL)
                    {
                        // cell is occupied
                        cost_map.data[index] = OCCUPIED_CELL;
                    }
                    else
                    {
                        // cell is not occupied
                        cost_map.data[index] = FREE_CELL;
                    }
                }
            }
        }

        break;
    }
    case USE_GRID_AND_ELEVATION_MAP:
    {
        // cost map based on elevation and grid map

        ROS_DEBUG("HectorCM: compute costmap based on grid and elevation map");

        int checksum_grid_map;

        // loop through each element
        for (int v = min_index(1); v < max_index(1); ++v)
        {
            for (int u = min_index(0); u < max_index(0); ++u)
            {
                unsigned int index = MAP_IDX(cost_map.info.width, u, v);

                // check if cell is known
                if(grid_map_.at<int8_t>(v,u) != UNKNOWN_CELL)
                {
                    if(grid_map_.at<int8_t>(v,u) == OCCUPIED_CELL || elevation_cost_map.data[index] == OCCUPIED_CELL)
                    {
                        checksum_grid_map = 0;

                        checksum_grid_map += grid_map_.at<int8_t>(v-1, u);
                        checksum_grid_map += grid_map_.at<int8_t>(v+1, u);
                        checksum_grid_map += grid_map_.at<int8_t>(v,   u-1);
                        checksum_grid_map += grid_map_.at<int8_t>(v,   u+1);
                        checksum_grid_map += grid_map_.at<int8_t>(v+1, u+1);
                        checksum_grid_map += grid_map_.at<int8_t>(v-1, u-1);
                        checksum_grid_map += grid_map_.at<int8_t>(v+1, u-1);
                        checksum_grid_map += grid_map_.at<int8_t>(v-1, u+1);

                        if(elevation_cost_map.data[index] == FREE_CELL && allow_elevation_map_to_clear_occupied_cells)
                        {
                            if(checksum_grid_map <= max_clear_size*OCCUPIED_CELL)
                            {
                                // cell is free
                                cost_map.data[index] = FREE_CELL;
                            }
                            else
                            {
                                // cell is occupied
                                cost_map.data[index] = OCCUPIED_CELL;
                            }
                        }
                        else
                        {
                            // cell is occupied
                            cost_map.data[index] = OCCUPIED_CELL;
                        }
                    }
                    else
                    {
                        cost_map.data[index] = FREE_CELL;
                    }
                }
            }
        }
        break;
    }
    case USE_GRID_AND_CLOUD_MAP:
    {
        // cost map based on cloud map and grid map

        ROS_DEBUG("HectorCM: compute costmap based on grid and cloud map");

        // loop through each element
        for (int v = min_index(1); v < max_index(1); ++v)
        {
            for (int u = min_index(0); u < max_index(0); ++u)
            {
                unsigned int index = MAP_IDX(cost_map.info.width, u, v);

                // check if cell is known
                if(grid_map_.at<int8_t>(v,u) != UNKNOWN_CELL)
                {
                    if(grid_map_.at<int8_t>(v,u) == OCCUPIED_CELL || cloud_cost_map.data[index] == OCCUPIED_CELL)
                    {
                            // cell is occupied
                            cost_map.data[index] = OCCUPIED_CELL;
                    }
                    else
                    {
                        cost_map.data[index] = FREE_CELL;
                    }
                }
            }
        }
        break;
    }

    case USE_GRID_AND_ELEVATION_MAP_AND_CLOUD_MAP:
    {
        // cost map based on elevation, cloud and grid map

        ROS_DEBUG("HectorCM: compute costmap based on grid, cloud and elevation map");

        int checksum_grid_map;

        // loop through each element
        for (int v = min_index(1); v < max_index(1); ++v)
        {
            for (int u = min_index(0); u < max_index(0); ++u)
            {
                unsigned int index = MAP_IDX(cost_map.info.width, u, v);

                // check if cell is known
                if(grid_map_.at<int8_t>(v,u) != UNKNOWN_CELL)
                {
                    if(grid_map_.at<int8_t>(v,u) == OCCUPIED_CELL || elevation_cost_map.data[index] == OCCUPIED_CELL)
                    {
                        checksum_grid_map = 0;

                        checksum_grid_map += grid_map_.at<int8_t>(v-1, u);
                        checksum_grid_map += grid_map_.at<int8_t>(v+1, u);
                        checksum_grid_map += grid_map_.at<int8_t>(v,   u-1);
                        checksum_grid_map += grid_map_.at<int8_t>(v,   u+1);
                        checksum_grid_map += grid_map_.at<int8_t>(v+1, u+1);
                        checksum_grid_map += grid_map_.at<int8_t>(v-1, u-1);
                        checksum_grid_map += grid_map_.at<int8_t>(v+1, u-1);
                        checksum_grid_map += grid_map_.at<int8_t>(v-1, u+1);

                        if(elevation_cost_map.data[index] == FREE_CELL && allow_elevation_map_to_clear_occupied_cells)
                        {
                            if(checksum_grid_map <= max_clear_size*OCCUPIED_CELL)
                            {
                                // cell is free
                                cost_map.data[index] = FREE_CELL;
                            }
                            else
                            {
                                // cell is occupied
                                cost_map.data[index] = OCCUPIED_CELL;
                            }
                        }
                        else
                        {
                            // cell is occupied
                            cost_map.data[index] = OCCUPIED_CELL;
                        }
                    }
                    else
                    {
                        if(cloud_cost_map.data[index] == OCCUPIED_CELL)
                        {
                            // cell is occupied
                            cost_map.data[index] = OCCUPIED_CELL;
                        }
                        else
                        {
                            // cell is free
                            cost_map.data[index] = FREE_CELL;
                        }

                    }
                }
            }
        }
        break;
    }
    }

    ROS_DEBUG("HectorCM: computed a new costmap");

    return true;
}
Ejemplo n.º 25
0
	void GSPipe::publish_stream() {
		ROS_INFO_STREAM("Publishing stream...");

		// Pre-roll camera if needed
		if (preroll_) {
		  ROS_DEBUG("Performing preroll...");

		  //The PAUSE, PLAY, PAUSE, PLAY cycle is to ensure proper pre-roll
		  //I am told this is needed and am erring on the side of caution.
		  gst_element_set_state(pipeline_, GST_STATE_PLAYING);
		  if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
			ROS_ERROR("Failed to PLAY during preroll.");
			return;
		  } else {
			ROS_DEBUG("Stream is PLAYING in preroll.");
		  }

		  gst_element_set_state(pipeline_, GST_STATE_PAUSED);
		  if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
			ROS_ERROR("Failed to PAUSE.");
			return;
		  } else {
			ROS_INFO("Stream is PAUSED in preroll.");
		  }
		}

		if(gst_element_set_state(pipeline_, GST_STATE_PLAYING) == GST_STATE_CHANGE_FAILURE) {
		  ROS_ERROR("Could not start stream!");
		  return;
		}
		ROS_INFO("Started stream.");

		// Poll the data as fast a spossible
		while(ros::ok()) {
		  // This should block until a new frame is awake, this way, we'll run at the
		  // actual capture framerate of the device.
		  ROS_DEBUG("Getting data...");
		  GstBuffer* buf = gst_app_sink_pull_buffer(GST_APP_SINK(sink_));


		  GstFormat fmt = GST_FORMAT_TIME;
		  gint64 current = -1;

		  // Query the current position of the stream
		  //if (gst_element_query_position(pipeline_, &fmt, &current)) {
			//ROS_INFO_STREAM("Position "<<current);
		  //}

		  // Stop on end of stream
		  if (!buf) {
			ROS_INFO("Stream ended.");
			break;
		  }

		  ROS_DEBUG("Got data.");

		  // Get the image width and height
		  GstPad* pad = gst_element_get_static_pad(sink_, "sink");
		  const GstCaps *caps = gst_pad_get_negotiated_caps(pad);
		  GstStructure *structure = gst_caps_get_structure(caps,0);
		  gst_structure_get_int(structure,"width",&width_);
		  gst_structure_get_int(structure,"height",&height_);

		  // Complain if the returned buffer is smaller than we expect
		  const unsigned int expected_frame_size =
			  image_encoding_ == sensor_msgs::image_encodings::RGB8
				  ? width_ * height_ * 3
				  : width_ * height_;

		  if (buf->size < expected_frame_size) {
			ROS_WARN_STREAM( "GStreamer image buffer underflow: Expected frame to be "
				<< expected_frame_size << " bytes but got only "
				<< (buf->size) << " bytes. (make sure frames are correctly encoded)");
		  }

		  // Construct Image message
		  sensor_msgs::ImagePtr img(new sensor_msgs::Image());
		  sensor_msgs::CameraInfoPtr cinfo;

		  // Update header information
		  cinfo.reset(new sensor_msgs::CameraInfo(camera_info_manager_.getCameraInfo()));
		  cinfo->header.stamp = ros::Time::now();
		  cinfo->header.frame_id = frame_id_;
		  img->header = cinfo->header;

		  // Image data and metadata
		  img->width = width_;
		  img->height = height_;
		  img->encoding = image_encoding_;
		  img->is_bigendian = false;
		  img->data.resize(expected_frame_size);

		  ROS_WARN("Image encoding: %s Width: %i Height %i \n", image_encoding_.c_str(), width_, height_);
		  // Copy only the data we received
		  // Since we're publishing shared pointers, we need to copy the image so
		  // we can free the buffer allocated by gstreamer
		  if (image_encoding_ == sensor_msgs::image_encodings::RGB8) {
			img->step = width_ * 3;
		  } else {
			img->step = width_;
		  }
		  std::copy(
			  buf->data,
			  (buf->data)+(buf->size),
			  img->data.begin());

		  // Publish the image/info
		  camera_pub_.publish(img, cinfo);

		  // Release the buffer
		  gst_buffer_unref(buf);

		  ros::spinOnce();
		}
	}
bool CostMapCalculation::calculateCostMap(char map_level)
{
    if (!map_level)
    {
      ROS_WARN("Invalid map selection was given to cost map calculation!");
      return false;
    }

    if (map_level & GRID_MAP) ROS_DEBUG("HectorCM: compute costmap based on grid map");
    if (map_level & ELEVATION_MAP) ROS_DEBUG("HectorCM: compute costmap based on elevation map");
    if (map_level & CLOUD_MAP) ROS_DEBUG("HectorCM: compute costmap based on cloud map");

    // loop through each element
    for (int v = min_index(1); v < max_index(1); ++v)
    {
        for (int u = min_index(0); u < max_index(0); ++u)
        {
            unsigned int index = MAP_IDX(cost_map.info.width, u, v);
            int checksum_grid_map = 0;

            cost_map.data[index] = UNKNOWN_CELL;

            // grid map
            if (map_level & GRID_MAP)
            {
                switch (grid_map_.data[index])
                {
                    case OCCUPIED_CELL:
                        if (map_level & ELEVATION_MAP && allow_elevation_map_to_clear_occupied_cells)
                        {
                            checksum_grid_map += grid_map_.at<int8_t>(v-1, u  );
                            checksum_grid_map += grid_map_.at<int8_t>(v+1, u  );
                            checksum_grid_map += grid_map_.at<int8_t>(v,   u-1);
                            checksum_grid_map += grid_map_.at<int8_t>(v,   u+1);
                            checksum_grid_map += grid_map_.at<int8_t>(v+1, u+1);
                            checksum_grid_map += grid_map_.at<int8_t>(v-1, u-1);
                            checksum_grid_map += grid_map_.at<int8_t>(v+1, u-1);
                            checksum_grid_map += grid_map_.at<int8_t>(v-1, u+1);
                        }

                        cost_map.data[index] = OCCUPIED_CELL;
                        break;
                    case FREE_CELL: cost_map.data[index] = FREE_CELL; break;
                }
            }

            // point cloud
            if (map_level & CLOUD_MAP)
            {
                if (cost_map.data[index] != OCCUPIED_CELL)
                {
                    switch (cloud_cost_map.data[index])
                    {
                        case OCCUPIED_CELL: cost_map.data[index] = OCCUPIED_CELL; break;
                        case FREE_CELL:     cost_map.data[index] = FREE_CELL;     break;
                    }
                }
            }

            // elevation map
            if (map_level & ELEVATION_MAP)
            {
                if (cost_map.data[index] != OCCUPIED_CELL || (allow_elevation_map_to_clear_occupied_cells && checksum_grid_map <= max_clear_size*OCCUPIED_CELL))
                {
                    switch (elevation_cost_map.data[index])
                    {
                        case OCCUPIED_CELL: cost_map.data[index] = OCCUPIED_CELL; break;
                        case FREE_CELL:     cost_map.data[index] = FREE_CELL;     break;
                    }
                }
            }
        }
    }

    ROS_DEBUG("HectorCM: computed a new costmap");

    return true;
}
Ejemplo n.º 27
0
void TeleopTurtle::keyLoop()
{
  char c;
  bool dirty=false;


  // get the console in raw mode                                                              
  tcgetattr(kfd, &cooked);
  memcpy(&raw, &cooked, sizeof(struct termios));
  raw.c_lflag &=~ (ICANON | ECHO);
  // Setting a new line, then end of file                         
  raw.c_cc[VEOL] = 1;
  raw.c_cc[VEOF] = 2;
  tcsetattr(kfd, TCSANOW, &raw);

  puts("Reading from keyboard");
  puts("---------------------------");
  puts("Up/Down : change speed by +/- 0.5 cm/s");
  puts("Left/Right : change steer by +/- 10 deg");

  linear_=angular_=0;

  for(;;)
  {
    // get the next event from the keyboard  
    if(read(kfd, &c, 1) < 0)
    {
      perror("read():");
      exit(-1);
    }

    ROS_DEBUG("value: 0x%02X\n", c);
  
    switch(c)
    {
      case KEYCODE_L:
        ROS_DEBUG("LEFT");
        angular_ -= 10.0;
        dirty = true;
        break;
      case KEYCODE_R:
        ROS_DEBUG("RIGHT");
        angular_ += 10.0;
        dirty = true;
        break;
      case KEYCODE_U:
        ROS_DEBUG("UP");
        linear_ += 0.5;
        dirty = true;
        break;
      case KEYCODE_D:
        ROS_DEBUG("DOWN");
        linear_ -= 0.5;
        dirty = true;
        break;
    case 's':
    case 'S':
      linear_ = angular_ = 0;
      dirty = true;
      break;
    }
   

    bb_msgs::DriveCmd vel;
    vel.steerAngle = a_scale_*angular_ * 3.141592654 / 180;
    vel.velocity = l_scale_*linear_;
    if(dirty ==true)
    {
      printf("velocity: %.1f cm/s\tsteer: %.1f deg\n", linear_, angular_);
      vel_pub_.publish(vel);    
      dirty=false;
    }
  }


  return;
}
Ejemplo n.º 28
0
Timer::Impl::~Impl()
{
  ROS_DEBUG("Timer deregistering callbacks.");
  stop();
}
Ejemplo n.º 29
0
void ScitosCharger::reconfigure_callback( scitos_mira::ChargerParametersConfig& config, uint32_t level) {
    ROS_DEBUG("Reconfigure request on ScitosCharger module.");
    //Set the MIRA parameters to what was selected...

    // TODO: Decide if this is a good idea: all parameters relate to contact control, which might be dangerous and not needed.
}
void CameraDisplay::updateCamera()
{
  sensor_msgs::CameraInfo::ConstPtr info;
  sensor_msgs::Image::ConstPtr image;
  {
    boost::mutex::scoped_lock lock( caminfo_mutex_ );

    info = current_caminfo_;
    image = texture_.getImage();
  }

  if( !info || !image )
  {
    return;
  }

  if( !validateFloats( *info ))
  {
    setStatus( StatusProperty::Error, "Camera Info", "Contains invalid floating point values (nans or infs)" );
    return;
  }

  Ogre::Vector3 position;
  Ogre::Quaternion orientation;
  context_->getFrameManager()->getTransform( image->header.frame_id, ros::Time(0), position, orientation );

  //printf( "CameraDisplay:updateCamera(): pos = %.2f, %.2f, %.2f.\n", position.x, position.y, position.z );

  // convert vision (Z-forward) frame to ogre frame (Z-out)
  orientation = orientation * Ogre::Quaternion( Ogre::Degree( 180 ), Ogre::Vector3::UNIT_X );

  float img_width = info->width;
  float img_height = info->height;

  // If the image width is 0 due to a malformed caminfo, try to grab the width from the image.
  if( img_width == 0 )
  {
    ROS_DEBUG( "Malformed CameraInfo on camera [%s], width = 0", qPrintable( getName() ));

    img_width = texture_.getWidth();
  }

  if (img_height == 0)
  {
    ROS_DEBUG( "Malformed CameraInfo on camera [%s], height = 0", qPrintable( getName() ));

    img_height = texture_.getHeight();
  }

  if( img_height == 0.0 || img_width == 0.0 )
  {
    setStatus( StatusProperty::Error, "Camera Info",
               "Could not determine width/height of image due to malformed CameraInfo (either width or height is 0)" );
    return;
  }

  double fx = info->P[0];
  double fy = info->P[5];

  float win_width = render_panel_->width();
  float win_height = render_panel_->height();
  float zoom_x = zoom_property_->getFloat();
  float zoom_y = zoom_x;

  // Preserve aspect ratio
  if( win_width != 0 && win_height != 0 )
  {
    float img_aspect = (img_width/fx) / (img_height/fy);
    float win_aspect = win_width / win_height;

    if ( img_aspect > win_aspect )
    {
      zoom_y = zoom_y / img_aspect * win_aspect;
    }
    else
    {
      zoom_x = zoom_x / win_aspect * img_aspect;
    }
  }

  // Add the camera's translation relative to the left camera (from P[3]);
  double tx = -1 * (info->P[3] / fx);
  Ogre::Vector3 right = orientation * Ogre::Vector3::UNIT_X;
  position = position + (right * tx);

  double ty = -1 * (info->P[7] / fy);
  Ogre::Vector3 down = orientation * Ogre::Vector3::UNIT_Y;
  position = position + (down * ty);

  if( !validateFloats( position ))
  {
    setStatus( StatusProperty::Error, "Camera Info", "CameraInfo/P resulted in an invalid position calculation (nans or infs)" );
    return;
  }

  render_panel_->getCamera()->setPosition( position );
  render_panel_->getCamera()->setOrientation( orientation );

  // calculate the projection matrix
  double cx = info->P[2];
  double cy = info->P[6];

  double far_plane = 100;
  double near_plane = 0.01;

  Ogre::Matrix4 proj_matrix;
  proj_matrix = Ogre::Matrix4::ZERO;
 
  proj_matrix[0][0]= 2.0 * fx/img_width * zoom_x;
  proj_matrix[1][1]= 2.0 * fy/img_height * zoom_y;

  proj_matrix[0][2]= 2.0 * (0.5 - cx/img_width) * zoom_x;
  proj_matrix[1][2]= 2.0 * (cy/img_height - 0.5) * zoom_y;

  proj_matrix[2][2]= -(far_plane+near_plane) / (far_plane-near_plane);
  proj_matrix[2][3]= -2.0*far_plane*near_plane / (far_plane-near_plane);

  proj_matrix[3][2]= -1;

  render_panel_->getCamera()->setCustomProjectionMatrix( true, proj_matrix );

  setStatus( StatusProperty::Ok, "Camera Info", "OK" );

#if 0
  static Axes* debug_axes = new Axes(scene_manager_, 0, 0.2, 0.01);
  debug_axes->setPosition(position);
  debug_axes->setOrientation(orientation);
#endif

  //adjust the image rectangles to fit the zoom & aspect ratio
  bg_screen_rect_->setCorners( -1.0f*zoom_x, 1.0f*zoom_y, 1.0f*zoom_x, -1.0f*zoom_y );
  fg_screen_rect_->setCorners( -1.0f*zoom_x, 1.0f*zoom_y, 1.0f*zoom_x, -1.0f*zoom_y );

  Ogre::AxisAlignedBox aabInf;
  aabInf.setInfinite();
  bg_screen_rect_->setBoundingBox( aabInf );
  fg_screen_rect_->setBoundingBox( aabInf );
}