Ejemplo n.º 1
0
    //! Loop forever while sending drive commands based on keyboard input
    bool driveKeyboard()
    {
        char cmd[50];
        m3ctrl_msgs::M3JointCmd humanoid_cmd;

        humanoid_cmd.chain.resize(1);
        humanoid_cmd.stiffness.resize(1);
        humanoid_cmd.position.resize(1);
        humanoid_cmd.velocity.resize(1);
        humanoid_cmd.effort.resize(1);
        humanoid_cmd.control_mode.resize(1);
        humanoid_cmd.smoothing_mode.resize(1);
        humanoid_cmd.chain_idx.resize(1);

        //  center robot first

        std::cout << "Example: commanding right arm J0.\n";
        std::cout << "Press any key to move to zero position.\n";
        std::cin.getline(cmd, 50);

        //humanoid_cmd.chain[0] = (unsigned char)RIGHT_ARM; // chain name: RIGHT_ARM, HEAD, RIGHT_HAND, LEFT_ARM, or LEFT_HAND
        humanoid_cmd.chain[0] = (unsigned char)LEFT_GRIPPER; // chain name: RIGHT_ARM, HEAD, or RIGHT_HAND
        humanoid_cmd.chain_idx[0] = 0; //J0
        humanoid_cmd.control_mode[0] = (unsigned char)JOINT_MODE_ROS_THETA; //Compliant position mode
        //humanoid_cmd.control_mode[0] = (unsigned char)JOINT_MODE_ROS_THETA; //Use for HEAD
        humanoid_cmd.smoothing_mode[0] = (unsigned char)SMOOTHING_MODE_SLEW; //Smooth trajectory
        //humanoid_cmd.smoothing_mode[0] = (unsigned char)SMOOTHING_MODE_MIN_JERK; //Use for HEAD
        humanoid_cmd.velocity[0] = 1.0; //Rad/s
        humanoid_cmd.stiffness[0] = 1.0; //0-1.0
        humanoid_cmd.effort[0] = 300.0; //Torque for hand fingers
        humanoid_cmd.position[0] = 0.5; //Desired position (Rad)
        humanoid_cmd.header.stamp = ros::Time::now();
        humanoid_cmd.header.frame_id = "humanoid_cmd";

//   printf("mo: %d\n",(int)humanoid_cmd.control_mode[0]);

        cmd_pub_.publish(humanoid_cmd);



        std::cout << "Type a command and then press enter.  "
                  "Use 'z' to go to middle, '+' to move up, '-' to move down, "
                  "'.' to exit.\n";

        while(nh_.ok()) {

            std::cin.getline(cmd, 50);
            if(cmd[0]!='+' && cmd[0]!='-' && cmd[0]!='z' && cmd[0]!='.')
            {
                std::cout << "unknown command:" << cmd << "\n";
                continue;
            }

            //move forward
            if(cmd[0]=='+') {
                //humanoid_cmd.position[0] += 5.0 * 3.14/180.;
                humanoid_cmd.position[0] += 2 * 3.14/180.;
            }
            //turn left (yaw) and drive forward at the same time
            else if(cmd[0]=='-') {
                //humanoid_cmd.position[0] -= 5 * 3.14/180.;
                humanoid_cmd.position[0] -= 2 * 3.14/180.;
            }
            //turn right (yaw) and drive forward at the same time
            else if(cmd[0]=='z') {
                humanoid_cmd.position[0] = 0 * 3.14/180.;
            }
            //quit
            else if(cmd[0]=='.') {
                break;
            }

            humanoid_cmd.header.stamp = ros::Time::now();
            //publish the assembled command
            cmd_pub_.publish(humanoid_cmd);
        }

        humanoid_cmd.header.stamp = ros::Time::now();
        cmd_pub_.publish(humanoid_cmd);
        humanoid_cmd.control_mode[0] = (unsigned char)JOINT_MODE_ROS_OFF; //Turn OFF

        return true;
    }
void LaserLegsCallback(const people_msgs::PositionMeasurementArray::ConstPtr& msg)
{
    bool validTrackLaser=false;

    cmd_vel.linear.x = 0.0;
    cmd_vel.angular.z = 0.0;

   nbOfTracksLaser=msg->people.size();

   if (nbOfTracksLaser>0) {
       //Extract coordinates of first detected person
       xLaserPerson=msg->people[0].pos.x;
       yLaserPerson=msg->people[0].pos.y;
//       ROS_INFO("xLaser: %f", xLaserPerson);
//       ROS_INFO("yLaser: %f", yLaserPerson);
//       ROS_INFO("AngleErrorLaser: %f", AngleErrorLaser);

        if (nbOfTracksKinect==0) {
       //Calculate angle error
       AngleErrorLaser=atan2(yLaserPerson,xLaserPerson);
       //Calculate distance error
       DistanceErrorLaser=sqrt(pow(xLaserPerson,2)+pow(yLaserPerson,2));

       YpathPointsLaser.insert(YpathPointsLaser.begin(),yLaserPerson);
       if (YpathPointsLaser.size()>6){
           yLast1Laser=YpathPointsLaser.at(5);
           yLast2Laser=YpathPointsLaser.at(1);
           xLast1Laser=xLaserPerson;
           tempDistanceLaser=DistanceErrorLaser;
           yDirectionLaser=yLast2Laser-yLast1Laser;
           YpathPointsLaser.pop_back();
       }

       if(!laser_obstacle_flag){
           angular_command=AngleErrorLaser*KpAngle;
           if(angular_command>MaxTurn){angular_command=MaxTurn;}
           if(angular_command<-MaxTurn){angular_command=-MaxTurn;}
           cmd_vel.angular.z = angular_command;

           double linearspeedLaser=(DistanceErrorLaser-DistanceTarget)*KpDistance;

           if (linearspeedLaser>MaxSpeed)
           {
               linearspeedLaser=MaxSpeed;
           }

           if (linearspeedLaser<0){
               linearspeedLaser=0;
           }
            cmd_vel.linear.x = linearspeedLaser;
           cmd_vel_pub.publish(cmd_vel);
       }
     }
        validTrackLaser=true;
        laserTrack=true;
//////////////////////////////////marker

        visualization_msgs::Marker marker;
        marker.header.frame_id = "base_link";
        marker.header.stamp = ros::Time();
        marker.ns = "laser";
        marker.id = 0;
        marker.type = visualization_msgs::Marker::SPHERE;
        marker.action = visualization_msgs::Marker::ADD;
        marker.pose.position.x = xLaserPerson;
        marker.pose.position.y = yLaserPerson;
        marker.pose.position.z = 0;
        marker.pose.orientation.x = 0.0;
        marker.pose.orientation.y = 0.0;
        marker.pose.orientation.z = 0.0;
        marker.pose.orientation.w = AngleErrorLaser;
        marker.scale.x = 0.1;
        marker.scale.y = 0.1;
        marker.scale.z = 0.1;
        marker.color.a = 1.0; // Don't forget to set the alpha!
        marker.color.r = 0.0;
        marker.color.g = 1.0;
        marker.color.b = 0.0;
        vis_pub1.publish( marker );

////////////////////////

  }
   else if(!validTrackKinect){
       laserTrack=false;
       //////////////////////////////////
/*       validTrackLaser=false;
       xPathLaser= xRobot+cos(orientationRobot+AngleErrorLaser)*tempDistanceLaser;
       yPathLaser= yRobot+sin(orientationRobot+AngleErrorLaser)*tempDistanceLaser;
       AngleErrorFollowLaser=PI-atan2(yPathLaser-yRobot,(xPathLaser-xRobot))-PI+orientationRobot;
           if(abs(AngleErrorFollowLaser)>PI){
               if(AngleErrorFollowLaser<0){AngleErrorFollowLaser=AngleErrorFollowLaser+2*PI;}
               else{AngleErrorFollowLaser=AngleErrorFollowLaser-2*PI;}
           }
           tempDistanceFollowLaser=sqrt(pow(xPathLaser-xRobot,2)+pow(yPathLaser-yRobot,2));

           //Get the number of tracks in the TrackArray
           nbOfTracksLaser=msg->people.size();

           //If at least 1 track, proceed
           if (nbOfTracksLaser>0) {
                validTrackLaser=true;
           }

           if (!laser_obstacle_flag){
               ros::Time start= ros::Time::now();
               while((ros::Time::now()-start<ros::Duration(round(tempDistanceFollowLaser/0.3))) && (!validTrackLaser) && (!laser_obstacle_flag)){
               cmd_vel.linear.x = 0.3;
               cmd_vel.angular.z=0.0;}

               cmd_vel.linear.x = 0.0;
               if(!validTrackLaser){
               if(yDirectionLaser>0){cmd_vel.angular.z=0.15;}
               else{cmd_vel.angular.z=-0.15;}
               }
           //Stop for loop
        //   validTrack=true;
           cmd_vel_pub.publish(cmd_vel);

//        ROS_INFO("xLast1: %f", xLast1);
//        ROS_INFO("yLast1: %f", yLast1);
//        ROS_INFO("yDirection: %f", yDirection);
   }
*/
       /////////////////////////////////////
   }
}
Ejemplo n.º 3
0
int main(int argc, char** argv) {

	// Initialize ourselves as a ROS node.
	ros::init(argc, argv, "planner");
	ros::NodeHandle nh;
	ros::Rate loop_rate(0.5); //hz
	vis = nh.advertise<visualization_msgs::Marker> ("visualization_marker", 0);

	// Subscribe to the map and people finder data.
	ros::Subscriber map_sub;
	ros::Subscriber people_finder_sub;
	ros::Subscriber robot_pose_sub;
	//ros::Subscriber costmap_sub;
	//costmap_sub = nh.subscribe<nav_msgs::GridCells>("/move_base_node/local_costmap/obstacles", 1, updateCost);
	map_sub = nh.subscribe<nav_msgs::OccupancyGrid> ("/map", 1, updateMap);
	people_finder_sub = nh.subscribe<hide_n_seek::PeopleFinder> (
			"fake_people_finder/people_finder", 1, updatePeople);
	robot_pose_sub = nh.subscribe<nav_msgs::Odometry> (
			"/base_pose_ground_truth", 1, updateRobotPose);

	// map_inited = false;
	pomdp.num_states = 400;
	pomdp.num_lookahead = 1;
	pomdp.states_inited = false;
	//pomdp.current_state.pose.position.x = 0;
	//pomdp.current_state.pose.position.y = 0;


	ROS_INFO("States initialized.");

	// spin the main loop
	while (ros::ok()) {
		ROS_INFO("service loop...");

		 {

			State internalGoal,personGoal;
			State s;
			if (people.size() != 0) {
				// If we have people to explore, exploring them is first priority.
				personGoal.pose = people.at(0);
				//this  is a real world pose, now convert it into one of the pompdp state to calculate the euc dist from all poss states
				personGoal.state_space_pose  = realToStatePose(personGoal.pose);
				s = makePlan(personGoal);
				ROS_INFO("Person found at real(%f, %f)! Using them as the next goal.", personGoal.pose.position.x, internalGoal.pose.position.y);
			} else if (pomdp.states_inited){
				// get goal from POMDp with highest probability, if its an internal state no conversion needed

				internalGoal = getMostLikelyState();
				s = makePlan(internalGoal);

			}
			//internalGoal.state_space_pose = realToStatePose(internalGoal.pose);
			// once we've initialized some states...
			// make a plan

			ROS_INFO("State received from makePlan: (%f, %f)", s.pose.position.x, s.pose.position.y);

			if (people.size() != 0) {
				people.erase(people.begin());
			}
			// print current goals
			printGoals(vis);
			// send a goal
			visualization_msgs::Marker marker;
			marker.header.frame_id = "map";//->header.frame_id;
			marker.id = 67845;
			marker.header.stamp = ros::Time();
			marker.type = visualization_msgs::Marker::ARROW;
			marker.action = visualization_msgs::Marker::ADD;
			marker.pose.position.x = s.pose.position.x;
			marker.pose.position.y = s.pose.position.y;
			marker.pose.position.z = 0;
			marker.pose.orientation.x = 0.0;
			marker.pose.orientation.y = -1.0;
			marker.pose.orientation.z = 0.0;
			marker.pose.orientation.w = 1.0;
			marker.scale.z = 2.0;
			marker.scale.x = 2.0;
			marker.scale.y = 2.0;

			marker.color.r = 1.0;
			marker.color.g = 1.0;
			marker.color.b = 1.0;
			vis.publish(marker);
			sendGoal(s);
			ROS_INFO("Goal we're sending: (%f, %f)", s.pose.position.x, s.pose.position.y);

			// set the current state
			pomdp.current_state = s;

		}

		ros::spinOnce();
		loop_rate.sleep();
	}
}
Ejemplo n.º 4
0
int main(int argc, char** argv){
  	
		//myfile.open ("/home/bojan/svn/POW/ROS/Fuerte/Data/Beds/User1/Final_pow_freek_3_2011-04-01-14-21-48.bag.txt");
    	//	myfile << "Yaw in deg" << "	" << "Velocity in m/s" << "	" << "Distance to bed in m" <<  "\n";
		ros::init(argc, argv, "my_tf_listener");	
		ros::NodeHandle node;

			//Visualize the BED
			marker_pub = node.advertise<visualization_msgs::Marker>("BED", 10);
			line.header.frame_id = "map";
	    		line.header.stamp = ros::Time::now();
	    		line.ns = "BED";
	    		line.type = visualization_msgs::Marker::LINE_STRIP;
	    		line.action = visualization_msgs::Marker::ADD;
	    		line.id = 0;

			    line.color.b = 1.0;
	    		line.color.a = 1.0;
	    		line.scale.x = 0.1;	//Sets the width of the LINE_STRIP
	   		 line.scale.y = 0.1;	//Ignored if marker type is LINE_STRIP
			    line.points.resize(2);
			


			//Visualize the PYLON1
			//marker_pub1 = node.advertise<visualization_msgs::Marker>("PYLON", 10);
			//cube1.header.frame_id = "map";
	    		//cube1.header.stamp = ros::Time::now();
	    		//cube1.ns = "PYLON";
	    		//cube1.type = visualization_msgs::Marker::CUBE;
	    		//cube1.action = visualization_msgs::Marker::ADD;
	    		//cube1.id = 1;

			//cube1.color.r = 1.0;
	    		//cube1.color.a = 1.0;
	    		//cube1.scale.x = 0.3;	//Sets the width of the LINE_STRIP
	   		//cube1.scale.y = 0.3;	//Ignored if marker type is LINE_STRIP
			//cube1.scale.z = 0.3;
		//	cube1.points.resize(2);
			

			//Visualize the PYLON2
			/*
			marker_pub2 = node.advertise<visualization_msgs::Marker>("PYLON", 10);
			cube2.header.frame_id = "map";
	    		cube2.header.stamp = ros::Time::now();
	    		cube2.ns = "PYLON";
	    		cube2.type = visualization_msgs::Marker::CUBE;
	    		cube2.action = visualization_msgs::Marker::ADD;
	    		cube2.id = 2;

			cube2.color.r = 1.0;
	    		cube2.color.a = 1.0;
	    		cube2.scale.x = 0.3;	//Sets the width of the LINE_STRIP
	   		cube2.scale.y = 0.3;	//Ignored if marker type is LINE_STRIP
			cube2.scale.z = 0.3;
			cube2.points.resize(2);

			//Visualize the PYLON3
			marker_pub3 = node.advertise<visualization_msgs::Marker>("PYLON", 10);
			cube3.header.frame_id = "map";
	    		cube3.header.stamp = ros::Time::now();
	    		cube3.ns = "PYLON";
	    		cube3.type = visualization_msgs::Marker::CUBE;
	    		cube3.action = visualization_msgs::Marker::ADD;
	    		cube3.id = 3;

			cube3.color.r = 1.0;
	    		cube3.color.a = 1.0;
	    		cube3.scale.x = 0.3;	//Sets the width of the LINE_STRIP
	   		cube3.scale.y = 0.3;	//Ignored if marker type is LINE_STRIP
			cube3.scale.z = 0.3;
			cube3.points.resize(2);

			//Visualize the PYLON4
			marker_pub4 = node.advertise<visualization_msgs::Marker>("PYLON", 10);
			cube4.header.frame_id = "map";
	    		cube4.header.stamp = ros::Time::now();
	    		cube4.ns = "PYLON";
	    		cube4.type = visualization_msgs::Marker::CUBE;
	    		cube4.action = visualization_msgs::Marker::ADD;
	    		cube4.id = 4;

			cube4.color.r = 1.0;
	    		cube4.color.a = 1.0;
	    		cube4.scale.x = 0.3;	//Sets the width of the LINE_STRIP
	   		cube4.scale.y = 0.3;	//Ignored if marker type is LINE_STRIP
			cube4.scale.z = 0.3;
			cube4.points.resize(2);
		*/

		tf::TransformListener listener;
  		ros::Rate rate(10.0);
		ros::Duration wait(5); wait.sleep();

		int speed_count = 0; double vel,yaw, xlast = 0 ,ylast = 0;
		
		while (node.ok())
			{

			//Speed calculation
			tf::StampedTransform transform2;
		if(speed_count==9)
		{
    			try{
      				//listener.lookupTransform("/map", "/scanmatcher_frame",  
      				listener.lookupTransform("/map", "/base_link", 
          ros::Time(0), transform2);
			
    			}
    				catch (tf::TransformException ex){};

    			vel = hypotenuse(xlast,ylast, transform2.getOrigin().x(),transform2.getOrigin().y());
			if(vel<0.003){
				yaw = tf::getYaw(transform2.getRotation());
			}

			speed_count = 0;
			xlast = transform2.getOrigin().x(); ylast = transform2.getOrigin().y();
		}
		else speed_count++;
				//Get the 4 corners of the wheelchair		

			tf::StampedTransform transform;
    			try{
      				listener.lookupTransform("/map", "/corner1",  
                               	ros::Time(0), transform);
			
    			}
    				catch (tf::TransformException ex){};
    			
			double x_vect_a = transform.getOrigin().x();
			double y_vect_a = transform.getOrigin().y();
			
    			
			try{
      				listener.lookupTransform("/map", "/corner2",  
                               	ros::Time(0), transform);
    			}
    				catch (tf::TransformException ex){};
    			
			double x_vect_b = transform.getOrigin().x();
			double y_vect_b = transform.getOrigin().y();
			
			try{
      				listener.lookupTransform("/map", "/corner3",  
                               	ros::Time(0), transform);
    			}
    				catch (tf::TransformException ex){};
    			
			double x_vect_c = transform.getOrigin().x();
			double y_vect_c = transform.getOrigin().y();

			try{
      				listener.lookupTransform("/map", "/corner4",  
                               	ros::Time(0), transform);
    			}
    				catch (tf::TransformException ex){};
    			
			double x_vect_d = transform.getOrigin().x();
			double y_vect_d = transform.getOrigin().y();

			//Find the minimum distance between the four line segments of the rectangle(wheelchair) and bed frame (line segment)
		
			//Line segment 1, between corner1 and corner2			
			//double p1x = -9.5; 
			//double p1y =  4.2;
			//double p2x = -9.5;
			//double p2y =  2.5;
			//hector
			double p1x = -3.3; 
			double p1y = -12.2;
			double p2x = -1.3;
			double p2y =  -11.75;			
			
			

			double p3x =  x_vect_a;
			double p3y =  y_vect_a;
			double p4x =  x_vect_b;
			double p4y =  y_vect_b;
			double distanceSegment1; 
			DistanceFromSegment(p1x, p1y, p2x, p2y ,p3x, p3y, p4x, p4y, distanceSegment1);
			
			//Line segment 2, between corner2 and corner3		
			double distanceSegment2; 
			DistanceFromSegment(p1x, p1y, p2x, p2y ,x_vect_b, y_vect_b, x_vect_c, y_vect_c, distanceSegment2);

			//Line segment 3, between corner3 and corner4			
			double distanceSegment3; 
			DistanceFromSegment(p1x, p1y, p2x, p2y ,x_vect_c, y_vect_c, x_vect_d, y_vect_d, distanceSegment3);
			
			//Line segment 4, between corner4 and corner1			
			double distanceSegment4; 
			DistanceFromSegment(p1x, p1y, p2x, p2y ,x_vect_d, y_vect_d, x_vect_a, y_vect_a, distanceSegment4);
			
			//Find the minimum distance of all 8 line to segment distances
			double distarray [4]= {distanceSegment1, distanceSegment2, distanceSegment3, distanceSegment4};
			
			double mindistance = distarray [0];
			for (int i = 1; i < 4; i++){
				if (distarray[i] < mindistance){
				    mindistance = distarray[i];
					 
								}
							}
					
			line.points[1].x = -3.3; line.points[1].y = -12.2;
			line.points[0].x = -1.3; line.points[0].y = -11.75; line.points[0].z = line.points[1].z = 0;
			//marker_pub.publish(line);
			
			
			//line.points[1].x = -3.5; line.points[1].y = -12.1;
			//line.points[0].x = -1.6; line.points[0].y = -11.7; line.points[0].z = line.points[1].z = 0;
			marker_pub.publish(line);
			
			
			//line.points[1].x = -8.7; line.points[1].y = 3.2;
			//line.points[0].x = -8.6; line.points[0].y = 1.4; line.points[0].z = line.points[1].z = 0;
			//marker_pub.publish(line);
			

			///PYLON
			//sphere.points[3].x = 0; cube.points[3].y = 0.5;
			//cube.points[2].x = 0.5; cube.points[2].y = 0.5; cube.points[0].z = cube.points[1].z = 0;
			//cube1.pose.position.x = -2.0; cube1.pose.position.y= -2.1;  cube1.pose.position.z = 1;

			//sphere.points[0].x = -3; sphere.points[0].y = 5; sphere.points[0].z = 5;
			/*
			sphere.pose.position.x = 1;
			sphere.pose.position.y = 1;
			sphere.pose.position.z = 0;
			sphere.pose.orientation.x = 0.0;
			sphere.pose.orientation.y = 0.0;
			sphere.pose.orientation.z = 0.0;
			sphere.pose.orientation.w = 0.0;
			*/

			
			//cube1.pose.position.x = -2.0; cube1.pose.position.y=  -2.1;  cube1.pose.position.z = 1;
		//	cube2.pose.position.x = -2.4; cube2.pose.position.y = -0.1;  cube2.pose.position.z = 1;
			//cube3.pose.position.x = -2.8; cube3.pose.position.y =  1.9;  cube3.pose.position.z = 1;
		//	cube4.pose.position.x = -3.2; cube4.pose.position.y =  3.9;  cube4.pose.position.z = 1;
		
			
			// U1R1
			/*
			cube1.pose.position.x = 1.5; cube1.pose.position.y = 0.9;  cube1.pose.position.z = 1;
			cube2.pose.position.x = 3.5; cube2.pose.position.y = 1.2;  cube2.pose.position.z = 1;
			cube3.pose.position.x = 5.5; cube3.pose.position.y = 1.5;  cube3.pose.position.z = 1;
			cube4.pose.position.x = 7.5; cube4.pose.position.y = 1.8;  cube4.pose.position.z = 1;	
			*/

			//U1R2
			/*
			cube1.pose.position.x = 1.1; cube1.pose.position.y = 1.1;  cube1.pose.position.z = 1;
			cube2.pose.position.x = 3.1; cube2.pose.position.y = 1.2;  cube2.pose.position.z = 1;
			cube3.pose.position.x = 5.1; cube3.pose.position.y = 1.3;  cube3.pose.position.z = 1;
			cube4.pose.position.x = 7.1; cube4.pose.position.y = 1.4;  cube4.pose.position.z = 1;
			*/			
			// U3R1
			/*
			cube1.pose.position.x = 1.9; cube1.pose.position.y = 1.1;  cube1.pose.position.z = 1;
			cube2.pose.position.x = 3.5; cube2.pose.position.y = 1.2;  cube2.pose.position.z = 1;
			cube3.pose.position.x = 5.3; cube3.pose.position.y = 1.3;  cube3.pose.position.z = 1;
			cube4.pose.position.x = 7.0; cube4.pose.position.y = 1.4;  cube4.pose.position.z = 1;	
			*/

			//marker_pub1.publish(cube1);
			//marker_pub2.publish(cube2);
			//marker_pub2.publish(cube3);
			//marker_pub2.publish(cube4);
					
					if(vel<0.0025){
					if (mindistance < 2) {
				ROS_INFO("Yaw: %f  Distance: %f",yaw*57.29 ,mindistance);
				//myfile << yaw*57.29 << " 	  " << vel <<  "        "  << mindistance <<  "\n";           
						    }
							}
				
			rate.sleep();
			}

  return 0;
};
/* handle a PointCloud2 message */
void pointcloud2_callback(sensor_msgs::PointCloud2 msg)
{
  ROS_INFO("Got PointCloud2 msg with %u points\n", msg.width * msg.height);

  if (!have_table){
    ROS_WARN("No table, bro\n");
    return;
  }
  // get the cloud in PCL format in the right coordinate frame
  PointCloudXYZ cloud, cloud2, full_cloud;
  msg.header.stamp = ros::Time(0);
  pcl::fromROSMsg(msg, cloud);
  if (!pcl_ros::transformPointCloud(target_frame, cloud, cloud2, *tf_listener)) {
    ROS_WARN("Can't transform point cloud; aborting object detection.");
    return;
  }
  else{
    cloud2.header.frame_id = target_frame;
    ROS_INFO("Changed the frame");
  }
  ROS_INFO("%s is the target_frame", target_frame.c_str());
  full_cloud = cloud2;
  ROS_DEBUG("full cloud is %zu points", full_cloud.points.size());
  /*for (size_t i = 0; i<table_polygon.points.size(); i++){
    ROS_INFO("%f %f %f",table_polygon.points[i].x, table_polygon.points[i].y, table_polygon.points[i].z);
    }*/

  // filter out points outside of the attention area, and only keep points above the table
  ROS_INFO("Have table, filtering points on table...");
  polygon_filter(table_polygon, cloud2, cloud);

  if (cloud.points.size() < 100) {
    ROS_WARN("Not enough points in table region. Only %zu points; aborting object detection.", cloud.points.size());
    return;
  }

  pcl::PassThrough<pcl::PointXYZ> pass;
  pass.setInputCloud(cloud.makeShared());
  pass.setFilterFieldName("z");
  float table_height = table_polygon.points[0].z;
  //float table_height = adjust_table_height(cloud);
  //this is where you set how much to chop off of the bottom of the objects. 
  //it's .02 right now..
  pass.setFilterLimits(table_height + .02, 1000.);
  pass.filter(cloud2);
  ROS_INFO("Done filtering table.");

  if (cloud2.points.size() < 100) {
    ROS_WARN("Not enough points above table. Only %zu points; aborting object detection.", cloud2.points.size());
    return;
  }
  ROS_INFO("Object Cloud is %zu points.", cloud2.points.size());
  // find cylinders
  ROS_INFO("%s is the cloud 2 frame right before CODE!!!!!!!!", cloud2.header.frame_id.c_str());
  cloud2.header.frame_id = target_frame;
  Cylinders C = find_cylinders(cloud2);

  cylinders_pub.publish(C); 
  ROS_INFO("Published cylinders msg\n");

}
Ejemplo n.º 6
0
void publishAidHUI(const ublox_msgs::AidHUI& m)
{
  static ros::Publisher publisher = nh->advertise<ublox_msgs::AidHUI>("aidhui", kROSQueueSize);
  publisher.publish(m);
}
Ejemplo n.º 7
0
void publishNavSOL(const ublox_msgs::NavSOL& m)
{
  static ros::Publisher publisher = nh->advertise<ublox_msgs::NavSOL>("navsol", kROSQueueSize);
  publisher.publish(m);
}
void VrepJoy::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{
	vrep_joy_pub_.publish(joy);
}
Ejemplo n.º 9
0
void RosAriaNode::publish()
{
  // Note, this is called via SensorInterpTask callback (myPublishCB, named "ROSPublishingTask"). ArRobot object 'robot' sholud not be locked or unlocked.
  pos = robot->getPose();
  tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(pos.getTh()*M_PI/180), tf::Vector3(pos.getX()/1000,
    pos.getY()/1000, 0)), position.pose.pose); //Aria returns pose in mm.
  position.twist.twist.linear.x = robot->getVel()/1000.0; //Aria returns velocity in mm/s.
  position.twist.twist.linear.y = robot->getLatVel()/1000.0;
  position.twist.twist.angular.z = robot->getRotVel()*M_PI/180;
  
  position.header.frame_id = frame_id_odom;
  position.child_frame_id = frame_id_base_link;
  position.header.stamp = ros::Time::now();
  pose_pub.publish(position);

  ROS_DEBUG("RosAria: publish: (time %f) pose x: %f, y: %f, angle: %f; linear vel x: %f, y: %f; angular vel z: %f", 
    position.header.stamp.toSec(), 
    (double)position.pose.pose.position.x,
    (double)position.pose.pose.position.y,
    (double)position.pose.pose.orientation.w,
    (double) position.twist.twist.linear.x,
    (double) position.twist.twist.linear.y,
    (double) position.twist.twist.angular.z
  );


  // publishing transform odom->base_link
  odom_trans.header.stamp = ros::Time::now();
  odom_trans.header.frame_id = frame_id_odom;
  odom_trans.child_frame_id = frame_id_base_link;
  
  odom_trans.transform.translation.x = pos.getX()/1000;
  odom_trans.transform.translation.y = pos.getY()/1000;
  odom_trans.transform.translation.z = 0.0;
  odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pos.getTh()*M_PI/180);
  
  odom_broadcaster.sendTransform(odom_trans);
  
  // getStallValue returns 2 bytes with stall bit and bumper bits, packed as (00 00 FrontBumpers RearBumpers)
  int stall = robot->getStallValue();
  unsigned char front_bumpers = (unsigned char)(stall >> 8);
  unsigned char rear_bumpers = (unsigned char)(stall);

  bumpers.header.frame_id = frame_id_bumper;
  bumpers.header.stamp = ros::Time::now();

  std::stringstream bumper_info(std::stringstream::out);
  // Bit 0 is for stall, next bits are for bumpers (leftmost is LSB)
  for (unsigned int i=0; i<robot->getNumFrontBumpers(); i++)
  {
    bumpers.front_bumpers[i] = (front_bumpers & (1 << (i+1))) == 0 ? 0 : 1;
    bumper_info << " " << (front_bumpers & (1 << (i+1)));
  }
  ROS_DEBUG("RosAria: Front bumpers:%s", bumper_info.str().c_str());

  bumper_info.str("");
  // Rear bumpers have reverse order (rightmost is LSB)
  unsigned int numRearBumpers = robot->getNumRearBumpers();
  for (unsigned int i=0; i<numRearBumpers; i++)
  {
    bumpers.rear_bumpers[i] = (rear_bumpers & (1 << (numRearBumpers-i))) == 0 ? 0 : 1;
    bumper_info << " " << (rear_bumpers & (1 << (numRearBumpers-i)));
  }
  ROS_DEBUG("RosAria: Rear bumpers:%s", bumper_info.str().c_str());
  
  bumpers_pub.publish(bumpers);

  //Publish battery information
  // TODO: Decide if BatteryVoltageNow (normalized to (0,12)V)  is a better option
  std_msgs::Float64 batteryVoltage;
  batteryVoltage.data = robot->getRealBatteryVoltageNow();
  voltage_pub.publish(batteryVoltage);

  if(robot->haveStateOfCharge())
  {
    std_msgs::Float32 soc;
    soc.data = robot->getStateOfCharge()/100.0;
    state_of_charge_pub.publish(soc);
  }

  // publish recharge state if changed
  char s = robot->getChargeState();
  if(s != recharge_state.data)
  {
    ROS_INFO("RosAria: publishing new recharge state %d.", s);
    recharge_state.data = s;
    recharge_state_pub.publish(recharge_state);
  }

  // publish motors state if changed
  bool e = robot->areMotorsEnabled();
  if(e != motors_state.data || !published_motors_state)
  {
	ROS_INFO("RosAria: publishing new motors state %d.", e);
	motors_state.data = e;
	motors_state_pub.publish(motors_state);
	published_motors_state = true;
  }

  // Publish sonar information, if enabled.
  if (publish_sonar || publish_sonar_pointcloud2)
  {
    sensor_msgs::PointCloud cloud;	//sonar readings.
    cloud.header.stamp = position.header.stamp;	//copy time.
    // sonar sensors relative to base_link
    cloud.header.frame_id = frame_id_sonar;
  

    std::stringstream sonar_debug_info; // Log debugging info
    sonar_debug_info << "Sonar readings: ";

    for (int i = 0; i < robot->getNumSonar(); i++) {
      ArSensorReading* reading = NULL;
      reading = robot->getSonarReading(i);
      if(!reading) {
        ROS_WARN("RosAria: Did not receive a sonar reading.");
        continue;
      }
    
      // getRange() will return an integer between 0 and 5000 (5m)
      sonar_debug_info << reading->getRange() << " ";

      // local (x,y). Appears to be from the centre of the robot, since values may
      // exceed 5000. This is good, since it means we only need 1 transform.
      // x & y seem to be swapped though, i.e. if the robot is driving north
      // x is north/south and y is east/west.
      //
      //ArPose sensor = reading->getSensorPosition();  //position of sensor.
      // sonar_debug_info << "(" << reading->getLocalX() 
      //                  << ", " << reading->getLocalY()
      //                  << ") from (" << sensor.getX() << ", " 
      //                  << sensor.getY() << ") ;; " ;
    
      //add sonar readings (robot-local coordinate frame) to cloud
      geometry_msgs::Point32 p;
      p.x = reading->getLocalX() / 1000.0;
      p.y = reading->getLocalY() / 1000.0;
      p.z = 0.0;
      cloud.points.push_back(p);
    }
    ROS_DEBUG_STREAM(sonar_debug_info.str());
    
    // publish topic(s)

    if(publish_sonar_pointcloud2)
    {
      sensor_msgs::PointCloud2 cloud2;
      if(!sensor_msgs::convertPointCloudToPointCloud2(cloud, cloud2))
      {
        ROS_WARN("Error converting sonar point cloud message to point_cloud2 type before publishing! Not publishing this time.");
      }
      else
      {
        sonar_pointcloud2_pub.publish(cloud2);
      }
    }

    if(publish_sonar)
    {
      sonar_pub.publish(cloud);
    }
  } // end if sonar_enabled
}
Ejemplo n.º 10
0
void* ControlThread(void *p)
{
	COdometerCapture *pComm = (COdometerCapture*)p;

	/// Handling events
	const int EVENT_NUM = 4;
	HANDLE hWait[EVENT_NUM];
	hWait[0] = pComm->m_hEventOdoCalTimer;
	hWait[1] = pComm->m_hEventOdoPubTimer;
	hWait[2] = pComm->m_hEventSpeedTimer;
	hWait[3] = pComm->m_hEventSpeedCheckTimer;

	/// Real Send Speed
//	double real_vx = 0.0;
//	double real_vy = 0.0;
//	double real_w = 0.0;
	// allowance change value for a single cycle
//	double allow_vx_change_per_cycle = 2.5;		// cm/s
//	double allow_vx_decrease_per_cycle = 5.75;    //cm/s
//	double allow_vy_change_per_cycle = 2.5;		// cm/s
//	double allow_w_increase_per_cycle = 4.5;		// deg/s
//	double allow_w_decrease_per_cycle = 4.5;		// deg/s
//
//	extern double g_closest_obstacle_x;  //urg看到的最近障碍物点 x,y坐标
//	extern double g_closest_obstacle_y;  //urg看到的最近障碍物点
//	extern double g_max_acc_inc_x;  //最大加速度
//	extern double g_max_acc_dec_x;  //最减加速度
//	extern double g_max_acc_w;
//	extern double g_max_vel_x; //最大速度
//	extern double g_max_vel_w; //最大角速度
	extern boost::mutex g_mutex_obstacle; //

//	double cur_ac_vx,cur_ac_vy,cur_ac_vw;
//	double old_x = 0,old_y = 0,old_w = 0;
	boost::posix_time::ptime prev_odom_time;
	boost::mutex vel_mutex;


	/// Handling loop
	fd_set fdset;
	//uint64_t exp;
	struct timeval timeout;
	timeout.tv_sec=5;
	timeout.tv_usec=0;
	while (pComm->m_initial)
	{
        FD_ZERO(&fdset);
        int maxfp=0;
	    for(int k=0;k<EVENT_NUM;k++)
	    {
	        FD_SET(hWait[k],&fdset);
	        if(maxfp<hWait[k])
               {
                    maxfp=hWait[k];
               }
	    }
	    int dRes = select(maxfp+1,&fdset,NULL,NULL,&timeout);
		switch (dRes)
		{
    //*********************************//
    //        修改case后面的值                     //
    //********************************//
        case -1 :
                cout<<"error of select"<<endl;
        case 0 :
                cout<<"No Data within five seconds"<<endl;
		default :
		    if(FD_ISSET(hWait[0],&fdset))						// 里程计合成
			{
				pComm->m_mutex_cap.Lock();
				ODOCAL->onTimerCal2(); /// Timer on calculation - using position
				pComm->m_mutex_cap.Unlock();
			}
            if(FD_ISSET(hWait[1],&fdset))						// 里程计发布
			{
				pComm->m_mutex_cap.Lock();
                nav_msgs::Odometry odometerData;
                pComm->GetCurOdometry(odometerData);
                odometerData.header.stamp = ros::Time::now();
                odometerData.header.frame_id = "odom";
                odometerData.child_frame_id = "base_link";
                static tf::TransformBroadcaster odom_broadcaster;
                geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(odometerData.pose.pose.orientation.z);
                ///first, we'll publish odom the transform over tf
                geometry_msgs::TransformStamped odom_trans;
                odom_trans.header.stamp = ros::Time::now();
                odom_trans.header.frame_id = "odom";
                odom_trans.child_frame_id = "base_link";

                odom_trans.transform.translation.x = odometerData.pose.pose.position.x;
                odom_trans.transform.translation.y = odometerData.pose.pose.position.y;
                odom_trans.transform.translation.z = 0.0;
                odom_trans.transform.rotation = odom_quat;
                ///send the transform
                odom_broadcaster.sendTransform(odom_trans);
                ///publish laser transform over tf
                ////sensor_msgs::LaserScan LaserData;
               // pComm->GetCurLaser(LaserData);
                static tf::TransformBroadcaster laser_broadcaster;
                tf::Transform laser_transform;
                laser_transform.setOrigin( tf::Vector3(0.3, 0, 0.05) );
                tf::Quaternion q;
                q.setRPY(0, 0, 0);
                laser_transform.setRotation(q);
                laser_broadcaster.sendTransform(tf::StampedTransform(laser_transform, ros::Time::now(), "base_link", "laser"));
//                geometry_msgs::Quaternion laser_quat = tf::createQuaternionMsgFromYaw(0);
//                geometry_msgs::TransformStamped laser_trans;
//                LaserData.header.stamp = ros::Time::now();
//                laser_trans.transform.translation.x = 0.3;
//                laser_trans.transform.translation.y = 0;
//                laser_trans.transform.translation.z = 0.05;
//                laser_trans.transform.rotation = laser_quat;
                //s_laser.publish(LaserData);
                m_odom.publish(odometerData);
				pComm->m_mutex_cap.Unlock();
			}
            if(FD_ISSET(hWait[2],&fdset))                    // 速度下发
			{
				geometry_msgs::Twist sendSpeed;
				pComm->GetCurSpeed(sendSpeed);
				double send_vx = sendSpeed.linear.x;
				double send_vy = sendSpeed.linear.y;
				double send_w = sendSpeed.angular.z;
				//限速
				send_vx = Utils::clip(send_vx,-1000.0,1000.0);
				send_vy = Utils::clip(send_vy,-1000.0,1000.0);
				send_w = Utils::clip(send_w,-50.0,50.0);
				MECANUM->SendVelocities(send_vx/100.0, send_vy/100.0, send_w*3.1415926/180.0);
				pComm->DoSafeSpeed();
			}
            if(FD_ISSET(hWait[3],&fdset))                    // 速度下发检查
			{
				pComm->SpeedCheck();
			}
			break;
		}
	}
	return 0;
}
Ejemplo n.º 11
0
void callback(const sensor_msgs::PointCloud2ConstPtr& pCloud)
{
	pcl::StopWatch watch;
	
	min_pt[0]=min_x - gridsize;
	min_pt[1]=min_y - gridsize;
	min_pt[2]=min_z - gridsize;
	
	max_pt[0]=max_x + gridsize;
	max_pt[1]=max_y + gridsize;
	max_pt[2]=max_z + gridsize;
	
	ball_grid_index[0] = 0;
	ball_grid_index[1] = 0;
	ball_grid_index[2] = 0;

	// new cloud formation 
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::fromROSMsg (*pCloud, *cloud);
	

	im_num++;

	if (first_run < 5)
	{
		
		first_run++;
	
		/////////////////////////////////////////
		// Remove points above given distance and below ground
		/////////////////////////////////////////
		for (unsigned int i=0;i<cloud->height;i++)
		{
			for (unsigned int j=0;j<cloud->width;j++)
			{
				unsigned int index_image = i * cloud->width + j;
				if (cloud->points[index_image].x > DEPTH_LIMIT)
				{
					cloud->points[index_image].x = sqrt(-1.0); // NaN
					cloud->points[index_image].y = sqrt(-1.0);
					cloud->points[index_image].z = sqrt(-1.0);
				}
			}
		}

		///////////////////////////////////////////////////////////////////////
		// Grid definition (counting number of points in each voxel of the grid)
		if (my_grid)
			my_grid->~CGrid_3D(); // destructor called here 
			my_grid = new CGrid_3D(gridsize,min_pt,max_pt); 
	
	}
	else
	{
		my_grid->reset();
	}
		
	my_grid->compute_grid(cloud);

	//////////////////////////////////////////////////////////////////////////////////////////////
	// Ball detection UFO
	ball_centre_coord3D[0]=0;
	ball_centre_coord3D[1]=0;
	ball_centre_coord3D[2]=0;
		
	my_grid->detect_ball(min_points, mask, MASKSIZE, ball_centre_coord3D, 
							ball_grid_index, 0, my_grid->xdim-MASKSIZE, 0, 
								my_grid->ydim-MASKSIZE, 2, my_grid->zdim-MASKSIZE, previous_ball);
	// Ball detection UFO
	//////////////////////////////////////////////////////////////////////////////////////////////
	
	dt=dt+watch.getTimeSeconds(); // running time instant 
	
	geometry_msgs::PointStamped point_out;
	
	if (ball_centre_coord3D[2] !=0)
	{	
		// coordinates published at time instant
		
		// KinectEstimated (KE) header stamp and frame id
		point_out.header.stamp = ros::Time::now();
		point_out.header.frame_id = "/ballCord";
		
		point_out.point.x = ball_centre_coord3D[0];
        point_out.point.y = ball_centre_coord3D[1];
        point_out.point.z = ball_centre_coord3D[2];
	
		ballCord.publish(point_out);

		printf("\n%3d - Flying ball - %6f %6f %6f",im_num,ball_centre_coord3D[0],ball_centre_coord3D[1],ball_centre_coord3D[2]);
	}
	else
	{
		// coordinates published at time instant
		
		// KinectEstimated (KE) header stamp and frame id
		point_out.header.stamp = ros::Time::now();
		point_out.header.frame_id = "/ballCord";
		
		point_out.point.x = 0.0;
        point_out.point.y = 0.0;
        point_out.point.z = 0.0;
	
		ballCord.publish(point_out);
	}
	// frequency print
    pcl::console::print_highlight ("\n frequency: %f\n", 1/(watch.getTimeSeconds()));
}
void 
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
//	std::cout << "\n\n----------------Received point cloud!-----------------\n";

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsampled (new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_planar (new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_objects (new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_red (new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_green (new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_blue (new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_yellow (new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_concat_clusters (new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_concat_hulls (new pcl::PointCloud<pcl::PointXYZRGB>);
	sensor_msgs::PointCloud2 downsampled2, planar2, objects2, filtered2, red2, green2, blue2, yellow2, concat_clusters2, concat_hulls2;
	std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> color_clouds, cluster_clouds, hull_clouds;
	std::vector<pcl::PointXYZRGB> labels;
	
//	fromROSMsg(*input, *cloud);
//	pub_input.publish(*input);

	// Downsample the input PointCloud
	pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
	sor.setInputCloud (input);
//	sor.setLeafSize (0.01, 0.01, 0.01);	//play around with leafsize (more samples, better resolution?)
	sor.setLeafSize (0.001, 0.001, 0.001);
	sor.filter (downsampled2);
	fromROSMsg(downsampled2,*downsampled);
	pub_downsampled.publish (downsampled2);	
	
	// Segment the largest planar component from the downsampled cloud
	pcl::SACSegmentation<pcl::PointXYZRGB> seg;
	pcl::ModelCoefficients::Ptr coeffs (new pcl::ModelCoefficients ());
	pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
	seg.setOptimizeCoefficients (true);
	seg.setModelType (pcl::SACMODEL_PLANE);
	seg.setMethodType (pcl::SAC_RANSAC);
	seg.setMaxIterations (100);
	seg.setDistanceThreshold (0.0085);
	seg.setInputCloud (downsampled);
	seg.segment (*inliers, *coeffs);
	
	// Extract the planar inliers from the input cloud
	pcl::ExtractIndices<pcl::PointXYZRGB> extract; 
	extract.setInputCloud (downsampled);
	extract.setIndices (inliers);
	extract.setNegative (false);
	extract.filter (*cloud_planar);
//	toROSMsg(*cloud_planar,planar2);
//	pub_planar.publish (planar2);

	// Remove the planar inliers, extract the rest
	extract.setNegative (true);
	extract.filter (*cloud_objects);
//	toROSMsg(*cloud_objects,objects2);
//	pub_objects.publish (objects2);

	// PassThrough Filter
	pcl::PassThrough<pcl::PointXYZRGB> pass;
	pass.setInputCloud (cloud_objects);
	pass.setFilterFieldName ("z");	//all duplos in pcd1
	pass.setFilterLimits (0.8, 1.0);
	pass.filter (*cloud_filtered);
	toROSMsg(*cloud_filtered,filtered2);
	pub_filtered.publish (filtered2);


//don't passthrough filter, does color filter work too? (cloud_red has many points in top right off the table)

	// Segment filtered PointCloud by color (red, green, blue, yellow)
	for (size_t i = 0 ; i < cloud_filtered->points.size () ; i++)
	{
		if ( (int(cloud_filtered->points[i].r) > 2*int(cloud_filtered->points[i].g)) && (cloud_filtered->points[i].r > cloud_filtered->points[i].b) )
			cloud_red->points.push_back(cloud_filtered->points[i]);
		if ( (cloud_filtered->points[i].g > cloud_filtered->points[i].r) && (cloud_filtered->points[i].g > cloud_filtered->points[i].b) )
			cloud_green->points.push_back(cloud_filtered->points[i]);
		if ( (cloud_filtered->points[i].b > cloud_filtered->points[i].r) && (cloud_filtered->points[i].b > cloud_filtered->points[i].g) )
			cloud_blue->points.push_back(cloud_filtered->points[i]);
		if ( (cloud_filtered->points[i].r > cloud_filtered->points[i].g) && (int(cloud_filtered->points[i].g) - int(cloud_filtered->points[i].b) > 30) )
			cloud_yellow->points.push_back(cloud_filtered->points[i]);
	}
	cloud_red->header.frame_id = "base_link";
	cloud_red->width = cloud_red->points.size ();
	cloud_red->height = 1;
	color_clouds.push_back(cloud_red);
	toROSMsg(*cloud_red,red2);
	pub_red.publish (red2);
	cloud_green->header.frame_id = "base_link";
	cloud_green->width = cloud_green->points.size ();
	cloud_green->height = 1;
	color_clouds.push_back(cloud_green);
	toROSMsg(*cloud_green,green2);
	pub_green.publish (green2);
	cloud_blue->header.frame_id = "base_link";
	cloud_blue->width = cloud_blue->points.size ();
	cloud_blue->height = 1;
	color_clouds.push_back(cloud_blue);
	toROSMsg(*cloud_blue,blue2);
	pub_blue.publish (blue2);
	cloud_yellow->header.frame_id = "base_link";
	cloud_yellow->width = cloud_yellow->points.size ();
	cloud_yellow->height = 1;
	color_clouds.push_back(cloud_yellow);
	toROSMsg(*cloud_yellow,yellow2);
	pub_yellow.publish (yellow2);
	

	// Extract Euclidian clusters from color-segmented PointClouds
	int j(0), num_red (0), num_green(0), num_blue(0), num_yellow(0);
	for (size_t cit = 0 ; cit < color_clouds.size() ; cit++)
	{
		pcl::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZRGB>);
		tree->setInputCloud (color_clouds[cit]);
		std::vector<pcl::PointIndices> cluster_indices;
		pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
		ec.setClusterTolerance (0.0075); // 0.01
//		ec.setMinClusterSize (12);
//		ec.setMaxClusterSize (75);
		ec.setMinClusterSize (100);
		ec.setMaxClusterSize (4000);
		ec.setSearchMethod (tree);
		ec.setInputCloud (color_clouds[cit]);
		ec.extract (cluster_indices);

		for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
		{	
			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
			for (std::vector<int>::const_iterator pit = it->indices.begin () ; pit != it->indices.end () ; pit++)
				cloud_cluster->points.push_back (color_clouds[cit]->points[*pit]);
			cloud_cluster->width = cloud_cluster->points.size ();
			cloud_cluster->height = 1;
			cloud_cluster->is_dense = true;
			cloud_cluster->header.frame_id = "base_link";
			cluster_clouds.push_back(cloud_cluster);
			labels.push_back(cloud_cluster->points[0]);
			if (cit == 0)	num_red++;
			if (cit == 1)	num_green++;
			if (cit == 2)	num_blue++;
			if (cit == 3)	num_yellow++;
			
			// Create ConvexHull for cluster (keep points on perimeter of cluster)
			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZRGB>);
  			pcl::ConvexHull<pcl::PointXYZRGB> chull;
  			chull.setInputCloud (cloud_cluster);
  			chull.reconstruct (*cloud_hull);
			cloud_hull->width = cloud_hull->points.size ();
			cloud_hull->height = 1;
			cloud_hull->header.frame_id = "base_link";
			hull_clouds.push_back(cloud_hull);

			j++;
		}
	}
	std::cout << "Number of RED clusters: " << num_red << std::endl;
	std::cout << "Number of GREEN clusters: " << num_green << std::endl;
	std::cout << "Number of BLUE clusters: " << num_blue << std::endl;
	std::cout << "Number of YELLOW clusters: " << num_yellow << std::endl;
	std::cout << "TOTAL number of clusters: " << j << std::endl;
	
	// Concatenate PointCloud clusters and convex hulls
	for (size_t k = 0 ; k < cluster_clouds.size() ; k++)
	{
		for (size_t l = 0 ; l < cluster_clouds[k]->size() ; l++)
		{
			cloud_concat_clusters->points.push_back(cluster_clouds[k]->points[l]);
			cloud_concat_hulls->points.push_back(hull_clouds[k]->points[l]);
		}
	}
	cloud_concat_clusters->header.frame_id = "base_link";
	cloud_concat_clusters->width = cloud_concat_clusters->points.size ();
	cloud_concat_clusters->height = 1;
	toROSMsg(*cloud_concat_clusters,concat_clusters2);
	pub_concat_clusters.publish (concat_clusters2);
	cloud_concat_hulls->header.frame_id = "base_link";
	cloud_concat_hulls->width = cloud_concat_hulls->points.size ();
	cloud_concat_hulls->height = 1;
	toROSMsg(*cloud_concat_hulls,concat_hulls2);
	pub_concat_hulls.publish (concat_hulls2);

	// Estimate the volume of each cluster
	double height, width;
	std::vector <double> heights, widths;
	std::vector <int>  height_ids, width_ids;
	for (size_t k = 0 ; k < cluster_clouds.size() ; k++)
	{
		// Calculate cluster height
		double tallest(0), shortest(1000), widest(0) ;
		for (size_t l = 0 ; l < cluster_clouds[k]->size() ; l++)
		{
			double point_to_plane_dist;
			point_to_plane_dist = 	   coeffs->values[0] * cluster_clouds[k]->points[l].x + 
						   coeffs->values[1] * cluster_clouds[k]->points[l].y +
						   coeffs->values[2] * cluster_clouds[k]->points[l].z + coeffs->values[3] / 
						   sqrt( pow(coeffs->values[0], 2) + pow(coeffs->values[1], 2)+ pow(coeffs->values[2], 2) );
			if (point_to_plane_dist < 0)		point_to_plane_dist = 0;
			if (point_to_plane_dist > tallest)	tallest = point_to_plane_dist;
			if (point_to_plane_dist < shortest)	shortest = point_to_plane_dist;
		}
		// Calculate cluster width
		for (size_t m = 0 ; m < hull_clouds[k]->size() ; m++)
		{	
			double parallel_vector_dist;
			for (size_t n = m ; n < hull_clouds[k]->size()-1 ; n++)
			{
				parallel_vector_dist = sqrt( pow(hull_clouds[k]->points[m].x - hull_clouds[k]->points[n+1].x,2) + 
  							     pow(hull_clouds[k]->points[m].y - hull_clouds[k]->points[n+1].y,2) +
							     pow(hull_clouds[k]->points[m].z - hull_clouds[k]->points[n+1].z,2) );
				if (parallel_vector_dist > widest)	widest = parallel_vector_dist;	
			}
		}
		// Classify block heights (error +/- .005m)
		height = (shortest < .01) ? tallest : tallest - shortest;	//check for stacked blocks	
		heights.push_back(height);
		if (height > .020 && height < .032)		height_ids.push_back(0);		//0: standing flat
		else if (height > .036 && height < .043)	height_ids.push_back(1);		//1: standing side
		else if (height > .064)				height_ids.push_back(2);		//2: standing long
		else						height_ids.push_back(-1);		//height not classified
		// Classify block widths (error +/- .005m)
		width = widest;
		widths.push_back(widest);
		if (width > .032-.005 && width < .0515+.005)		width_ids.push_back(1);		//1: short
		else if (width > .065-.005 && width < .0763+.005)	width_ids.push_back(2);		//2: medium
		else if (width > .1275-.005 && width < .1554+.005)	width_ids.push_back(4);		//4: long
		else						width_ids.push_back(-1);		//width not classified
	}

	// Classify block size using width information
	std::vector<int> block_ids, idx_1x1, idx_1x2, idx_1x4, idx_unclassified;
	int num_1x1(0), num_1x2(0), num_1x4(0), num_unclassified(0);
	for (size_t p = 0 ; p < width_ids.size() ; p++)	
	{
		if (width_ids[p] == 1)		
		{
			block_ids.push_back(1);		//block is 1x1
			idx_1x1.push_back(p);
			num_1x1++;
		}
		else if (width_ids[p] == 2)	
		{
			block_ids.push_back(2);         //block is 1x2
			idx_1x2.push_back(p);
			num_1x2++;
		}
		else if (width_ids[p] == 4)     
		{
			block_ids.push_back(4);         //block is 1x4
			idx_1x4.push_back(p);
			num_1x4++;
		}
		else	
		{
			block_ids.push_back(-1);	//block not classified
			idx_unclassified.push_back(p);
			num_unclassified++;
		}
	}

	// Determine Duplos of the same size
	std::cout << "\nThere are " << num_1x1 << " blocks of size 1x1 ";
	if (num_1x1>0)	std::cout << "(cluster index: ";
	for (size_t q = 0 ; q < idx_1x1.size() ; q++)
		std::cout << idx_1x1[q] << ", ";
	if (num_1x1>0)  std::cout << ")";
	std::cout << "\nThere are " << num_1x2 << " blocks of size 1x2 ";
        if (num_1x2>0)  std::cout << "(cluster index: ";
	for (size_t q = 0 ; q < idx_1x2.size() ; q++)
		std::cout << idx_1x2[q] << ", ";
	if (num_1x2>0)  std::cout << ")";
	std::cout << "\nThere are " << num_1x4 << " blocks of size 1x4 ";
        if (num_1x4>0)  std::cout << "(cluster index: ";
	for (size_t q = 0 ; q < idx_1x4.size() ; q++)
		std::cout << idx_1x4[q] << ", ";
	if (num_1x4>0)  std::cout << ")";
	std::cout << "\nThere are " << num_unclassified << " unclassified blocks ";
        if (num_unclassified>0)  std::cout << "(cluster index: ";
	for (size_t q = 0 ; q < idx_unclassified.size() ; q++)
		std::cout << idx_unclassified[q] << ", ";
	if (num_unclassified>0)  std::cout << ")";
	std::cout << "\n\n\n";
	return;
}
Ejemplo n.º 13
0
void SvmDetect::CloudClustersCallback(const lidar_tracker::CloudClusterArray::Ptr& in_cloud_cluster_array_ptr)
{
	cloud_clusters_pub_.publish(*in_cloud_cluster_array_ptr);
}
Ejemplo n.º 14
0
bool robot(unit::for_double::Request &req, unit::for_double::Response &res)
{

	
	ros::Time begin = ros::Time::now();
	  
	pos_robotx= req.positionx;
	pos_roboty= req.positiony;
	ang_robot = req.angle ;



	check = true ;
	msg.data = conv;
	chatter_pub.publish(msg); 	
	



	setCurrentOdeContext(0);
	restoreOdeState(0);
	const dReal *pos_first = odeBodyGetPosition(body1);
	
	odeBodySetPosition(body1,pos_robotx,pos_roboty,pos_first[2]);
	const dReal *pos_second = odeBodyGetPosition(body1);
	printf(" Pos X %f  Pos Y %f\n", pos_second[0], pos_second[1]);


	
	saveOdeState(0);
	


	//init all trajectories to the master state
	//init all trajectories to the master state
		for (int i=0; i<nSamples; i++)
		{
			//activate the context for this sample
			setCurrentOdeContext(i+1); 
			//load the state from the master context
			restoreOdeState(0);


			const dReal *pos = odeBodyGetPosition(body1);
		odeBodySetPosition(body1,pos_robotx,pos_roboty,pos[2]);
			const dReal *pos1 = odeBodyGetPosition(body1);

	
			saveOdeState(i+1,0); 


		}

		for (int i=0; i<nSamples; i++)
		{
			setCurrentOdeContext(i);
			const dReal *pos = odeBodyGetPosition(body1);
	      //  printf(" position x :%f y:%f z:%f NUM: %d  \n",pos[0],pos[1],pos[2],i);
		}
		
		
		//signal the start of new C-PBP iteration
		setCurrentOdeContext(0); 
		restoreOdeState(0); 


		const dReal *pos2 = odeBodyGetPosition(body1);

		odeBodySetPosition(body1,pos_robotx,pos_roboty,pos2[2]);




		const dReal *pos = odeBodyGetPosition(body1);
		float angle=odeJointGetHingeAngle(joint2);

		//printf(" posX x :%f, aVel: %f  \n",pos[0],aVel);
		float stateVector[2]={pos[0],pos[1]};
		pbp.startIteration(true,stateVector);

		

		//simulate forward 
		for (int k=0; k<nTimeSteps; k++)

		{


			//signal the start of a planning step
			pbp.startPlanningStep(k);
						//NOTE: for multithreaded operation, one would typically run each iteration of the following loop in a separate thread. 
			//The getControl(), getPreviousSampleIdx(), and updateResults() methods of the optimizer are thread-safe.
			//The physics simulation is also thread-safe as we have full copies of the simulated system for each thread/trajectory

			for (int i=0; i<nSamples; i++)
			{
				//get control from C-PBP
				float control, control1;
				pbp.getControl(i,&control);
				pbp.getControl(i,&control1);

				//printf("k=%i",k);
				//printf("i=%d \n",i);
				//printf("control: %f  control 1 %f\n",control,control1);


				//get the mapping from this to previous state (affected by resampling operations)
				int previousStateIdx=pbp.getPreviousSampleIdx(i);
				//simulate to get next state.
				setCurrentOdeContext(i+1);
				//printf("previous state id: %d \n",previousStateIdx);
				restoreOdeState(previousStateIdx+1); //continue the a trajectory (selected in the resampling operation inside ControlPBP)

				pos = odeBodyGetPosition(body1);
				angle=odeJointGetHingeAngle(joint2);
		// printf("before Posx %1.3f,posz = %f  avel %1.3f, \n",pos[0],pos[2],(aVel*180/PI));
		 // printf("control %f \n",control);
				//dVector3 torque={0,control,0};
				//odeBodyAddTorque(body,torque);


 
    dReal MaxForce = dInfinity;
	odeJointSetHingeParam(joint2,dParamFMax,dInfinity); 
	odeJointSetHingeParam(joint2,dParamVel,control  );

	odeJointSetHingeParam(joint1,dParamFMax,dInfinity); 
	odeJointSetHingeParam(joint1,dParamVel,control1  );

				stepOde(1);

				pos = odeBodyGetPosition(body1);
				angle=odeJointGetHingeAngle(joint2);
		//	printf("AFTER Posx %1.3f,posz = %f  avel %1.3f,\n",pos[0],pos[2],aVel);


				const dReal *pos = odeBodyGetPosition(body1);
				float angle=odeJointGetHingeAngle(joint2);
				float angle1=odeJointGetHingeAngle(joint1);
			float cost=squared((0.03-pos[0])*200.0f)+squared((0.01-pos[1])*200.0f) ;//+ squared(angle*10.1f)+squared(angle1*10.1f) ;//+ squared(control* 1.5f)+ squared(control1*1.5f);

			/*

			if ( pos[0]<0.06 && pos[0]>0.03 )
			{
				
				if( pos[1]>-0.03 && pos[0]<0.03  )

			{
				cost = cost + 10000;
			}
			}
			*/


				//store the state and cost to C-PBP. Note that in general, the stored state does not need to contain full simulation state as in						this simple case.
				//instead, one may use arbitrary state features
				float stateVector[2]={pos[0],pos[1]};
				float cont[2]= {control,control1};

				//float stateVector[2]={angle,aVel};
				pbp.updateResults(i,cont,stateVector,cost);

				//printf("cost in the loop end: %f posz: %f \n",cost,pos[2]);
			}
//save all states, will be used at next step (the restoreOdeState() call above)
	for (int i=0; i<nSamples; i++)
			{
				saveOdeState(i+1,i+1);
			}

			//signal the end of the planning step. this normalizes the state costs etc. for the next step
			pbp.endPlanningStep(k);
		}
			//signal the end of an iteration. this also executes the backwards smoothing pass
		pbp.endIteration();
		
		//deploy the best control found using the master context
		float control[2];
		pbp.getBestControl(0,control);
		
		float cost=(float)pbp.getSquaredCost();
		//printf("Cost %f \n",cost);
		
	
		
		setCurrentOdeContext(0);
		restoreOdeState(0);

		//for(int j=0; j<20; j++)
		//{




	    if (cnt >3)
	    {
	    	
           
    	dReal MaxForce = dInfinity;
		odeJointSetHingeParam(joint2,dParamFMax,dInfinity);
		odeJointSetHingeParam(joint2,dParamVel,control[0]);

		odeJointSetHingeParam(joint1,dParamFMax,dInfinity);
		odeJointSetHingeParam(joint1,dParamVel,control[1]);

		}

		else 
		{
			
		odeJointSetHingeParam(joint2,dParamFMax,dInfinity);
		odeJointSetHingeParam(joint2,dParamVel,0.0);

		odeJointSetHingeParam(joint1,dParamFMax,dInfinity);
		odeJointSetHingeParam(joint1,dParamVel,0.0);


		}

		stepOde(0);
		saveOdeState(0);

		//print output, both angle and aVel should converge to 0, with some sampling noise remaining

	  	 pos = odeBodyGetPosition(body1);
		//angle=odeJointGetHingeAngle(hinge);
	   	angle=odeJointGetHingeAngle(joint2);


        res.commandx = control[0];
        res.commandy = control[1];
		conv = angle ;

		last_angle = angle;

		cnt = cnt +1 ;
		last_position = pos[0] ;
		printf("cnt %d\n",cnt );


	

		ros::Time end = ros::Time::now();
		double dt = (begin - end).toSec();

		//ROS_INFO("dt %f", dt);

		
		printf("FINAL Posx %1.3f,posy = %f  angle %1.3f,  control %f,  control 1 %f \n",pos[0],pos[1],angle*180/3.1416,control[0],control[1]);
	  	// printf("angle %1.3f, avel %1.3f, cost=%1.3f\n",angle,aVel,cost);

		
	/*	int i = 0;
		loop : cin >> i;
		if ( i != 1)
		goto loop;
	*/
	
}
Ejemplo n.º 15
0
void publishAidALM(const ublox_msgs::AidALM& m)
{
  static ros::Publisher publisher = nh->advertise<ublox_msgs::AidALM>("aidalm", kROSQueueSize);
  publisher.publish(m);
}
Ejemplo n.º 16
0
void Mapper::processCloud(unique_ptr<DP> newPointCloud, const std::string& scannerFrame, const ros::Time& stamp, uint32_t seq)
{
	processingNewCloud = true;
	BoolSetter stopProcessingSetter(processingNewCloud, false);

	// if the future has completed, use the new map
	processNewMapIfAvailable();
	
	// IMPORTANT:  We need to receive the point clouds in local coordinates (scanner or robot)
	timer t;
	
	// Convert point cloud
	const size_t goodCount(newPointCloud->features.cols());
	if (goodCount == 0)
	{
		ROS_ERROR("I found no good points in the cloud [at mapper.cpp]");
		return;
	}
	
	// Dimension of the point cloud, important since we handle 2D and 3D
	const int dimp1(newPointCloud->features.rows());

	ROS_INFO("Processing new point cloud");
	{
		timer t; // Print how long take the algo
		
		// Apply filters to incoming cloud, in scanner coordinates
		inputFilters.apply(*newPointCloud);
		
		ROS_INFO_STREAM("Input filters took " << t.elapsed() << " [s]");
	}
	
	string reason;
	// Initialize the transformation to identity if empty
 	if(!icp.hasMap())
 	{
		// we need to know the dimensionality of the point cloud to initialize properly
		publishLock.lock();
		TOdomToMap = PM::TransformationParameters::Identity(dimp1, dimp1);
		publishLock.unlock();
	}

	// Fetch transformation from scanner to odom
	// Note: we don't need to wait for transform. It is already called in transformListenerToEigenMatrix()
	PM::TransformationParameters TOdomToScanner;
	try
	{
		TOdomToScanner = PointMatcher_ros::eigenMatrixToDim<float>(
				PointMatcher_ros::transformListenerToEigenMatrix<float>(
				tfListener,
				scannerFrame,
				odomFrame,
				stamp
			), dimp1);
	}
	catch(tf::ExtrapolationException e)
	{
		ROS_ERROR_STREAM("Extrapolation Exception. stamp = "<< stamp << " now = " << ros::Time::now() << " delta = " << ros::Time::now() - stamp);
		return;
	}

	//Correct TOdomToMap to fit dimp1 (dimension of input data)
	//We need to do this because we could have called 'CorrectPose' which always returns a [4][4] matrix
	TOdomToMap = PointMatcher_ros::eigenMatrixToDim<float>(TOdomToMap, dimp1);

	ROS_DEBUG_STREAM("TOdomToScanner(" << odomFrame<< " to " << scannerFrame << "):\n" << TOdomToScanner);
	ROS_DEBUG_STREAM("TOdomToMap(" << odomFrame<< " to " << mapFrame << "):\n" << TOdomToMap);
		
 	const PM::TransformationParameters TscannerToMap = transformation->correctParameters(TOdomToMap * TOdomToScanner.inverse());
	ROS_DEBUG_STREAM("TscannerToMap (" << scannerFrame << " to " << mapFrame << "):\n" << TscannerToMap);
	
	// Ensure a minimum amount of point after filtering
	const int ptsCount = newPointCloud->features.cols();
	if(ptsCount < minReadingPointCount)
	{
		ROS_ERROR_STREAM("Not enough points in newPointCloud: only " << ptsCount << " pts.");
		return;
	}

	// Initialize the map if empty
 	if(!icp.hasMap())
 	{
		ROS_INFO_STREAM("Creating an initial map");
		mapCreationTime = stamp;
		setMap(updateMap(newPointCloud.release(), TscannerToMap, false));
		// we must not delete newPointCloud because we just stored it in the mapPointCloud
		return;
	}
	
	// Check dimension
	if (newPointCloud->features.rows() != icp.getInternalMap().features.rows())
	{
		ROS_ERROR_STREAM("Dimensionality missmatch: incoming cloud is " << newPointCloud->features.rows()-1 << " while map is " << icp.getInternalMap().features.rows()-1);
		return;
	}

	// Call this function in order to verbose errors when matching the newPointCloud and the icp Internal Map
	debugFeaturesDescriptors(*newPointCloud);

	try 
	{
		// Apply ICP
		PM::TransformationParameters Ticp;
		Ticp = icp(*newPointCloud, TscannerToMap);

		ROS_DEBUG_STREAM("Ticp:\n" << Ticp);
		
		// Ensure minimum overlap between scans
		const double estimatedOverlap = icp.errorMinimizer->getOverlap();
		ROS_INFO_STREAM("Overlap: " << estimatedOverlap);
		if (estimatedOverlap < minOverlap)
		{
			ROS_ERROR_STREAM("Estimated overlap too small, ignoring ICP correction!");
			return;
		}
		
		// Compute tf
		publishStamp = stamp;
		publishLock.lock();
		TOdomToMap = Ticp * TOdomToScanner;
		// Publish tf
		tfBroadcaster.sendTransform(PointMatcher_ros::eigenMatrixToStampedTransform<float>(TOdomToMap, mapFrame, odomFrame, stamp));
		publishLock.unlock();
		processingNewCloud = false;
 		
		ROS_DEBUG_STREAM("TOdomToMap:\n" << TOdomToMap);

 		// Publish odometry
		if (odomPub.getNumSubscribers())
			odomPub.publish(PointMatcher_ros::eigenMatrixToOdomMsg<float>(Ticp, mapFrame, stamp));
	
		// Publish error on odometry
		if (odomErrorPub.getNumSubscribers())
			odomErrorPub.publish(PointMatcher_ros::eigenMatrixToOdomMsg<float>(TOdomToMap, mapFrame, stamp));

		// Publish outliers
		if (outlierPub.getNumSubscribers())
		{
			//DP outliers = PM::extractOutliers(transformation->compute(*newPointCloud, Ticp), *mapPointCloud, 0.6);
			//outlierPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(outliers, mapFrame, mapCreationTime));
		}

		// check if news points should be added to the map
		if (
			mapping &&
			((estimatedOverlap < maxOverlapToMerge) || (icp.getInternalMap().features.cols() < minMapPointCount)) &&
			#if BOOST_VERSION >= 104100
			(!mapBuildingInProgress)
			#else // BOOST_VERSION >= 104100
			true
			#endif // BOOST_VERSION >= 104100
		)
		{
			// make sure we process the last available map
			mapCreationTime = stamp;
			#if BOOST_VERSION >= 104100
			ROS_INFO("Adding new points to the map in background");
			mapBuildingTask = MapBuildingTask(boost::bind(&Mapper::updateMap, this, newPointCloud.release(), Ticp, true));
			mapBuildingFuture = mapBuildingTask.get_future();
			mapBuildingThread = boost::thread(boost::move(boost::ref(mapBuildingTask)));
			mapBuildingInProgress = true;
			#else // BOOST_VERSION >= 104100
			ROS_INFO("Adding new points to the map");
			setMap(updateMap( newPointCloud.release(), Ticp, true));
			#endif // BOOST_VERSION >= 104100
		}
	}
	catch (PM::ConvergenceError error)
	{
		ROS_ERROR_STREAM("ICP failed to converge: " << error.what());
		return;
	}
	
	//Statistics about time and real-time capability
	int realTimeRatio = 100*t.elapsed() / (stamp.toSec()-lastPoinCloudTime.toSec());
	ROS_INFO_STREAM("[TIME] Total ICP took: " << t.elapsed() << " [s]");
	if(realTimeRatio < 80)
		ROS_INFO_STREAM("[TIME] Real-time capability: " << realTimeRatio << "%");
	else
		ROS_WARN_STREAM("[TIME] Real-time capability: " << realTimeRatio << "%");

	lastPoinCloudTime = stamp;
}
Ejemplo n.º 17
0
void publishAidEPH(const ublox_msgs::AidEPH& m)
{
  static ros::Publisher publisher = nh->advertise<ublox_msgs::AidEPH>("aideph", kROSQueueSize);
  publisher.publish(m);
}
bool setImageCropLimitsCB(apc_msgs::SetImageCrop::Request &req, apc_msgs::SetImageCrop::Response &res)
{
	double 	focalLength_x, 	focalLength_y;
	double 	bin_x_min, 		bin_x_max, 		bin_y_min, 	bin_y_max, 	bin_z_min, 	bin_z_max;
	int   	pix_x_min, 		pix_x_max, 		pix_y_min, 	pix_y_max;

	ROS_INFO("Set Crop Service Called! :D");

	// Calculate Focal Lengths
	focalLength_x 	= 	640 / (2 * (tan((57/2) * (M_PI/180))));
	focalLength_y 	= 	480 / (2 * (tan((43/2) * (M_PI/180))));
//	focalLength_x	= 	610;
//	focalLength_y 	= 	610;

	ROS_INFO("Focal length: %d, %d", (int)focalLength_x, (int)focalLength_x);
	// Get Bin Details

//	double dy 		= 	0.1;
//	double dz 		= 	0.1;
//
//	double w2 		= 	req.width / 2;
//	double h2 		= 	req.height / 2;

//	bin_x_min       =   (req.center.x - w2);
//	bin_y_min       =   (req.center.y + h2);
//	bin_z_min       =   req.center.z  + 0.2;
//
//	bin_x_max       =   (req.center.x + w2);
//	bin_y_max       =   (req.center.y - h2);
//	bin_z_max       =   req.center.z  + 0.5;

	bin_x_min       =   (req.o.x);
	bin_y_min       =   (req.o.y) - .26;	//TODO figure out the reason for the bias in Y direction
	bin_z_min       =   req.o.z  + 0.2;

	bin_x_max       =   (req.b.x);
	bin_y_max       =   (req.b.y) - .26;	//TODO figure out the reason for the bias in Y direction
	bin_z_max       =   req.b.z  + 0.5;

	ROS_INFO("The Bin min coordinate is: %f, %f, %f  and The Bin max coordinate is: %f, %f, %f", bin_x_min, bin_y_min, bin_z_min, bin_x_max, bin_y_max, bin_z_max);

	// Pixel Calculation
	pix_x_min 		= 	((bin_x_min * focalLength_x)/bin_z_min) + 320 - 20;
	pix_y_min 		= 	((bin_y_min * focalLength_y)/bin_z_min) + 240 + 20;

	pix_x_max 		= 	((bin_x_max * focalLength_x)/bin_z_min) + 320 + 20;
	pix_y_max 		= 	((bin_y_max * focalLength_y)/bin_z_min) + 240 - 20;

	ROS_INFO("The Pixels are: %d, %d, %d, %d", pix_x_min, pix_y_min, pix_x_max, pix_y_max);

	// Publishing corner markers
	markerArray.markers.resize(4);

	publishBinCornerMarker(req.o, 0);
	publishBinCornerMarker(req.a, 1);
	publishBinCornerMarker(req.b, 2);
	publishBinCornerMarker(req.c, 3);

	pub_marker_bin_corner_.publish(markerArray);


	// Sanity check
	if((pix_x_min < 0) | (pix_y_min < 0) | ((pix_x_min + abs(pix_x_max - pix_x_min)) > 640) | ((pix_y_min + abs(pix_y_max - pix_y_min))>480))
	{
		return true;
	}
	// Update Filter
	roi_x 	  		= 	pix_x_min;
	roi_y 	  		= 	pix_y_min;
	roi_width 		= 	abs(pix_x_max - pix_x_min);
	roi_height		= 	abs(pix_y_max - pix_y_min);

	ROS_INFO("The Filter is: %d, %d, %d, %d", roi_x, roi_y, roi_width, roi_height);
	// Enable Crop
	//cropped 		= 	req.crop_image;

	res.success 	= 	true;



	return true;

}
Ejemplo n.º 19
0
void publish(const MessageT& m, const std::string& topic) {
  static ros::Publisher publisher = nh->advertise<MessageT>(topic, kROSQueueSize);
  publisher.publish(m);
}
Ejemplo n.º 20
0
int main(int argc, char** argv)
{
    ros::init(argc, argv, "IMU_Node2");
    ros::NodeHandle n;


     imu_pub = n.advertise<sensor_msgs::Imu>("/imu/data",100);


    // Change the next line according to your port name and baud rate
    try
    {

        if(device.isOpen())
           {
               ROS_INFO("Port is opened");
          }
    }
    catch(serial::SerialException& e)
    {
        ROS_FATAL("Failed to open the serial port!!!");
        ROS_BREAK();
    }

       ros::Rate r(50);
          while(ros::ok())
          {


       // Get the reply, the last value is the timeout in ms
       try{
     std::string reply;

        // ros::Duration(0.005).sleep();

           device.readline(reply);
           Roll = strtod(reply.c_str(),&pRoll)/2;
           Pitch = strtod(pRoll,&pRoll)/2;
           Yaw  = strtod(pRoll,&pRoll)/2;
           GyroX = strtod(pRoll,&pRoll);
           GyroY = strtod(pRoll,&pRoll);
           GyroZ = strtod(pRoll,&pRoll);
           AccX = strtod(pRoll,&pRoll);
           AccY = strtod(pRoll,&pRoll);
           AccZ = strtod(pRoll,&pRoll);
           Mx = strtod(pRoll,&pRoll);
           My = strtod(pRoll,&pRoll);
           Mz = strtod(pRoll,NULL);

           ROS_INFO("Euler %.2f %.2f %.2f",Roll,Pitch,Yaw);

	   sensor_msgs::Imu imu;

           geometry_msgs::Quaternion Q;
           Q = tf::createQuaternionMsgFromRollPitchYaw(Roll*-1,Pitch*-1,Yaw*-1);

           imu.orientation.x = 0;
           imu.orientation.y = 0;
           imu.orientation.z = 0;
           imu.orientation.w = 1;


           LinearX =  (AccX/256)*9.806;
           LinearY = (AccY/256)*9.806;
           LinearZ = (AccZ/256)*9.806;

           imu.angular_velocity.x = GyroX*-1*0.07*0.01745329252;;
           imu.angular_velocity.y = GyroY*0.07*0.01745329252;;
           imu.angular_velocity.z = GyroZ*-1*0.07*0.01745329252;;

           imu.linear_acceleration.x =  LinearX*-1;
           imu.linear_acceleration.y =  LinearY;
           imu.linear_acceleration.z =  LinearZ*-1;
           imu.header.stamp = ros::Time::now();
           imu.header.frame_id = "imu";
           imu_pub.publish(imu);


       }
       catch(serial::SerialException& e)
       {
           ROS_INFO("Error %s",e.what());
       }

              r.sleep();
    }

}
/**
 * This function takes the Hand data published by the RightHandRobotics hand
 * and sends that information in joint format to an rviz visualizer
 */
void publish_to_rviz(const reflex_msgs::HandConstPtr& hand)
{
    // Pressure sensor setup
    bool contact_val;
    float pressure_val;
    visualization_msgs::MarkerArray marker_array;

    // Finger joint assignment
    joint_state.header.stamp = ros::Time::now();
    joint_state.position[0] = hand->finger[0].proximal;
    joint_state.position[1] = hand->finger[1].proximal;
    joint_state.position[2] = hand->finger[2].proximal;
    joint_state.position[3] = hand->palm.preshape;
    joint_state.position[4] = -hand->palm.preshape;

    int index = num_fixed_steps;
    for (int finger = 0; finger<3; finger++)
    {
        for (int i=0; i<(num_flex_steps+1); i++)
        {
            joint_state.position[index] = hand->finger[finger].distal/((float) (num_flex_steps+1));
            index++;
        }
    }

    // Pressure sensor assignment
    for (int finger=0; finger<3; finger++)
    {
        char prox_fid[20];
        sprintf(prox_fid, "/proximal_%d_tactile", (finger+1));
        char dist_fid[20];
        sprintf(dist_fid, "/distal_%d_tactile", (finger+1));

        for (int i=0; i<sensors_per_finger; i++)    // Loop through tactile sensors in the fingers
        {
            contact_val = hand->finger[finger].contact[i];
            pressure_val = hand->finger[finger].pressure[i];
            visualization_msgs::Marker contact_marker = makeContactMarker(contact_val, i, 0.004, 0.005, true);
            visualization_msgs::Marker pressure_marker = makePressureMarker(pressure_val, i, 0.008, 0.003, true);
            if (i < 5) {    // Proximal link
                contact_marker.header.frame_id = prox_fid;
                pressure_marker.header.frame_id = prox_fid;
            } else {      // Distal link
                contact_marker.header.frame_id = dist_fid;
                pressure_marker.header.frame_id = dist_fid;
            }
            contact_marker.id = i + (finger*sensors_per_finger);
            pressure_marker.id = i + (finger*sensors_per_finger);
            marker_array.markers.push_back(contact_marker);
            marker_array.markers.push_back(pressure_marker);
        }
    }

    // Loop through tactile sensors in the palm
    for (int i=0; i<11; i++)
    {
        contact_val = hand->palm.contact[palm_map[i]];
        pressure_val = hand->palm.pressure[palm_map[i]];
        visualization_msgs::Marker contact_marker = makeContactMarker(contact_val, i, 0.004, 0.01, false);
        visualization_msgs::Marker pressure_marker = makePressureMarker(pressure_val, i, 0.009, 0.008, false);
        marker_array.markers.push_back(contact_marker);
        marker_array.markers.push_back(pressure_marker);
    }

    // Publish joint data
    joint_pub.publish(joint_state);
    // Publish sensor markers
    sensor_pub.publish(marker_array);
}
Ejemplo n.º 22
0
void publishNavSVINFO(const ublox_msgs::NavSVINFO& m)
{
  static ros::Publisher publisher = nh->advertise<ublox_msgs::NavSVINFO>("navsvinfo", kROSQueueSize);
  publisher.publish(m);
}
Ejemplo n.º 23
0
int main(int argc, char**argv){
	
	//initialize ROS
	ros::init(argc, argv, "main");
	
	//create a nodehandle
	ros::NodeHandle nh;
	
	//use handle to subscribe to messages (name, buffer, callback pointer)
	ros::Subscriber skeleton_sub = nh.subscribe("/skeletons", 1, skeletonCallback);
	
	//if this doesn't work, send it to the Converter without Braitenburg
	//gesture_pub = nh.advertise<MotorControlMsg::MotorCommand>("Motor_Command", 1000);
	gesture_pub = nh.advertise<robot_msgs::MotorData>("motordata",200);
	
	ros::Rate loop_rate(10);
	while(ros::ok()){
		//all spinning conditions met, send the command
		if(sendSpin){
			/*
			
			This was the version that was to be sent to Braitenburg.
			
			MotorControlMsg::MotorCommand GestureMsg;
		
			GestureMsg.precedence = -1;
			GestureMsg.isLeftRightVel = true;
			GestureMsg.linear_velocity = 0.25;
			GestureMsg.angular_velocity = -0.25;
		
			gesture_pub.publish(GestureMsg);
			sendSpin = false;
			*/
			
			//create message to send to Converter
			robot_msgs::MotorData GestureMsg;
	
			GestureMsg.motor_left_velocity = 25;
			GestureMsg.motor_left_time = 100;
			GestureMsg.motor_right_velocity = -25;
			GestureMsg.motor_right_time = 100;
			
			//publish message 5 times because of fix in Converter for Braitenburg
			for(int i=0; i<100; i++){
				gesture_pub.publish(GestureMsg);
			}
			
			sendSpin = false;
			spinSent = true;
			
			ROS_INFO("SUCCESS!!! Motor Command sent.");
		}
		
		else if(stopSpin){
		
			robot_msgs::MotorData GestureMsg;
			
			//send zeroes to stop the spinning
			GestureMsg.motor_left_velocity = 0;
			GestureMsg.motor_left_time = 100;
			GestureMsg.motor_right_velocity = 0;
			GestureMsg.motor_right_time = 100;
			
			//publish message 5 times because of fix in Converter for Braitenburg
			for(int i=0; i<10; i++){
				gesture_pub.publish(GestureMsg);
			}
			
			stopSpin = false;
			
			ROS_INFO("SUCCESS!!! Motor Command sent.");
		}
	
		//give callback time to respond
		ros::spinOnce();
		
		loop_rate.sleep();
		ROS_INFO("End of loop reached.");
	}
}
Ejemplo n.º 24
0
void publishNavCLK(const ublox_msgs::NavCLOCK& m)
{
  static ros::Publisher publisher = nh->advertise<ublox_msgs::NavCLOCK>("navclock", kROSQueueSize);
  publisher.publish(m);
}
void poseCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
    xRobot=msg->pose.pose.position.x;
    yRobot=msg->pose.pose.position.y;
    //  orientationRobot=msg->pose.pose.orientation.z;
    tf::Pose pose;
    tf::poseMsgToTF(msg->pose.pose, pose);
    orientationRobot= tf::getYaw(pose.getRotation());  //get radiand rotation (0 front, 3.14 back, left positive, right negative)
//    ROS_INFO("xRobot: %f", xRobot);
//    ROS_INFO("yRobot: %f", yRobot);
//    ROS_INFO("orientationRobot: %f", orientationRobot);

    ROS_INFO("xRobot: %f", xRobot);
    ROS_INFO("yRobot: %f", yRobot);
//    ROS_INFO("orientationRobot: %f", orientationRobot);
    ROS_INFO("BigLeft: %d", BigLeft);
    ROS_INFO("SmallLeft: %d", SmallLeft);
    ROS_INFO("WallLeft: %d", WallLeft);
    ROS_INFO("BigRight: %d", BigRight);
    ROS_INFO("SmallRight: %d", SmallRight);
    ROS_INFO("WallRight: %d", WallRight);
    ROS_INFO("laser_obstacle_flag: %d", laser_obstacle_flag);
//    ROS_INFO("laser_angular_velocity: %f", laser_angular_velocity);
    ROS_INFO("xLaser: %f", xLaserPerson);
    ROS_INFO("yLaser: %f", yLaserPerson);
//    ROS_INFO("AngleErrorLaser: %f", AngleErrorLaser);
//    ROS_INFO("KinectLaserError: %f", error);
    ROS_INFO("match: %d", kinectLaserMatch);
    ROS_INFO("Confidence: %f", confidence);
    ROS_INFO("Height: %f", height);
    ROS_INFO("distanceKinect: %f", distanceKinect);
//    ROS_INFO("age: %f", age);
//    ROS_INFO("AngleErrorKinect: %f", AngleErrorKinect);
    ROS_INFO("AngleErrorPan: %f", (AngleErrorPan*180)/PI);
    ROS_INFO("xKinect: %f", xperson);
    ROS_INFO("yKinect: %f", yperson);
//    ROS_INFO("AngleErrorFollow: %f", AngleErrorFollow);
    ROS_INFO("xPath: 0");
    ROS_INFO("yPath: 0");
    ROS_INFO("xFollow: 0");
    ROS_INFO("yFollow: 0");
    ROS_INFO("tempDistance: %f", tempDistance);
//    ROS_INFO("xLast1: %f", xLast1);
//    ROS_INFO("yLast1: %f", yLast1);
//    ROS_INFO("yDirection: %f", yDirection);
    ROS_INFO("kinectTrack: %d", kinectTrack);
    ROS_INFO("laserTrack: %d", laserTrack);




    //////////////////////////////////marker
            for(int i=0;i<100000;i++){
            visualization_msgs::Marker marker;
            marker.header.frame_id = "odom";
            marker.header.stamp = ros::Time();
            marker.ns = "robotPose";
            marker.id = i;
            marker.type = visualization_msgs::Marker::SPHERE;
            marker.action = visualization_msgs::Marker::ADD;
            marker.lifetime=ros::Duration(100.0);
            marker.pose.position.x = xRobot;
            marker.pose.position.y = yRobot;
            marker.pose.position.z = 0;
            marker.pose.orientation.x = 0.0;
            marker.pose.orientation.y = 0.0;
            marker.pose.orientation.z = 0.0;
         //   marker.pose.orientation.w = orientationRobot;
            marker.pose.orientation.w = 1.0;
            marker.scale.x = 0.2;
            marker.scale.y = 0.2;
            marker.scale.z = 0.2;
            marker.color.a = 1.0; // Don't forget to set the alpha!
            marker.color.r = 1.0;
            marker.color.g = 0.0;
            marker.color.b = 0.0;
            vis_pub3.publish( marker );
            }
    ////////////////////////
}
Ejemplo n.º 26
0
void publishRxmRAW(const ublox_msgs::RxmRAW& m)
{
  static ros::Publisher publisher = nh->advertise<ublox_msgs::RxmRAW>("rxmraw", kROSQueueSize);
  publisher.publish(m);
}
void personCallback(const opt_msgs::TrackArray::ConstPtr& msg)
{
    validTrackKinect=false;


    //Initialize the twist
    cmd_vel.linear.x = 0.0;
    cmd_vel.angular.z = 0.0;

    //Get the number of tracks in the TrackArray
    nbOfTracksKinect=msg->tracks.size();

    //If at least 1 track, proceed
    if (nbOfTracksKinect>0) {
        //looping throught the TrackArray
        for(int i=0;i<nbOfTracksKinect && !validTrackKinect;i++){
            //oldest track which is older than the age threshold and above the confidence threshold
            if ((msg->tracks[i].age>AgeThreshold) && (msg->tracks[i].confidence>ConfidenceTheshold) && (msg->tracks[i].height>HeightTheshold) && (msg->tracks[i].height<HeightMaxTheshold)){

                distanceKinect=msg->tracks[i].distance;
                xperson=((msg->tracks[i].distance)*cos(AngleSmallError+AngleErrorPan));
                yperson=((msg->tracks[i].distance)*sin(AngleSmallError+AngleErrorPan));
                AngleErrorKinect=atan2(yperson,xperson);
                age=msg->tracks[i].age;
                height=msg->tracks[i].height;
                confidence=msg->tracks[i].confidence;

                YpathPoints.insert(YpathPoints.begin(),yperson);
                if (YpathPoints.size()>5){
                    yLast1=YpathPoints.at(4);
                    yLast2=YpathPoints.at(1);
                    xLast1=xperson;
                    tempDistanceKinect=distanceKinect;
                    yDirection=yLast2-yLast1;
                    YpathPoints.pop_back();
                }



                error= sqrt(pow(xperson-xLaserPerson,2)+pow(yperson-yLaserPerson,2));  //calculate the x and y error between the kinect and the laser
//                ROS_INFO("KinectLaserError: %f", error);

                if (error<0.2){
                    kinectLaserMatch=true;
//                    ROS_INFO("match: %d", kinectLaserMatch);
                }else{kinectLaserMatch=false;}

                //Calculate distance error
                DistanceError=distanceKinect-DistanceTarget;

                //print to the console
//                ROS_INFO("Confidence: %f", msg->tracks[i].confidence);
//                ROS_INFO("Height: %f", msg->tracks[i].height);
//                ROS_INFO("distanceKinect: %f", distanceKinect);
//                ROS_INFO("age: %f", msg->tracks[i].age);
//                ROS_INFO("AngleErrorKinect: %f", AngleErrorKinect);
//                ROS_INFO("AngleErrorPan: %f", (AngleErrorPan*180)/ PI);

//                ROS_INFO("xperson: %f", xperson);
//                ROS_INFO("yperson: %f", yperson);

                //Set command Twist
          //      if(smallError==false ){  //to reduce vibration around 0 angle of the kinect view, and 0 robot angle  // && abs(AngleErrorPan)<0.05
          //    cmd_vel.angular.z = (AngleErrorPan+followingAngle)*KpAngle;
                if (!laser_obstacle_flag){
                    angular_command= AngleErrorKinect*KpAngle;;
                    if(abs(followingAngle)>0.1 && abs(AngleErrorKinect)<0.2){angular_command = (AngleErrorKinect+followingAngle)*KpAngleOcclusion;}
                 //   else {angular_command = AngleErrorKinect*KpAngle;}

                        if(angular_command>MaxTurn){angular_command=MaxTurn;}
                        if(angular_command<-MaxTurn){angular_command=-MaxTurn;}
                    cmd_vel.angular.z = angular_command;


           //     if(abs(followingAngle)<0.1){cmd_vel.angular.z = (AngleErrorKinect+followingAngle)*KpAngle;}
           //     else {cmd_vel.angular.z = (AngleErrorKinect+followingAngle)*KpAngleOcclusion;}

             //   cmd_vel.angular.z = AngleError*KpAngle;
            //    }
                //Avoid going backward
                if (DistanceError>0.05){  //threshold for small distance error of 0.05 meter
                    double command_speed=DistanceError*KpDistance;
                    //Limit the speed
                    if (command_speed>MaxSpeed){command_speed=MaxSpeed;}
                    if (command_speed<0){command_speed=0;}
                    cmd_vel.linear.x = command_speed;
                }
                //Stop for loop
                validTrackKinect=true;
                cmd_vel_pub.publish(cmd_vel);
             }
        }
    }

    kinectTrack=true;
    //////////////////////////////////marker
            visualization_msgs::Marker marker;
            marker.header.frame_id = "base_link";
            marker.header.stamp = ros::Time();
            marker.ns = "kinect";
            marker.id = 0;
            marker.type = visualization_msgs::Marker::SPHERE;
            marker.action = visualization_msgs::Marker::ADD;
            marker.pose.position.x = xperson;
            marker.pose.position.y = yperson;
            marker.pose.position.z = 0;
            marker.pose.orientation.x = 0.0;
            marker.pose.orientation.y = 0.0;
            marker.pose.orientation.z = 0.0;
            marker.pose.orientation.w = AngleErrorKinect;
            marker.scale.x = 0.1;
            marker.scale.y = 0.1;
            marker.scale.z = 0.1;
            marker.color.a = 1.0; // Don't forget to set the alpha!
            marker.color.r = 0.0;
            marker.color.g = 1.0;
            marker.color.b = 1.0;
            vis_pub2.publish( marker );

    ////////////////////////
    }

    else if(!validTrackLaser){
            kinectTrack=false;
  //          validTrackKinect=false;
            xPath= xRobot+cos(orientationRobot+AngleErrorKinect)*tempDistanceKinect;
            yPath= yRobot+sin(orientationRobot+AngleErrorKinect)*tempDistanceKinect;
            AngleErrorFollow=PI-atan2(yPath-yRobot,(xPath-xRobot))-PI+orientationRobot;
                if(abs(AngleErrorFollow)>PI){
                    if(AngleErrorFollow<0){AngleErrorFollow=AngleErrorFollow+2*PI;}
                    else{AngleErrorFollow=AngleErrorFollow-2*PI;}
                }
                tempDistance=sqrt(pow(xPath-xRobot,2)+pow(yPath-yRobot,2));

                //Get the number of tracks in the TrackArray
                nbOfTracksKinect=msg->tracks.size();

/*                //If at least 1 track, proceed
                if (nbOfTracksKinect>0) {
                    //looping throught the TrackArray
                    for(int i=0;i<nbOfTracksKinect && !validTrackKinect;i++){
                        //oldest track which is older than the age threshold and above the confidence threshold
                        if ((msg->tracks[i].age>AgeThreshold) && (msg->tracks[i].confidence>ConfidenceTheshold) && (msg->tracks[i].height>HeightTheshold) && (msg->tracks[i].height<HeightMaxTheshold)){
                        validTrackKinect=true;
                        }
                    }
                }
*/
                if (!laser_obstacle_flag){
                    ros::Time start= ros::Time::now();                   
                    while((ros::Time::now()-start<ros::Duration(round(tempDistance/0.3))) && (!validTrackKinect) && (!laser_obstacle_flag)){
                    if (!laser_obstacle_flag)
                        {cmd_vel.linear.x = 0.3;
                        cmd_vel.angular.z=0.0;}
                    else{cmd_vel.angular.z = laser_angular_velocity;
                        cmd_vel.linear.x = laser_linear_velocity;}
                    }

                    cmd_vel.linear.x = 0.0;
                    if(!validTrackKinect){
                    if(yDirection>0){cmd_vel.angular.z=0.2;}
                    else{cmd_vel.angular.z=-0.2;}
                    }
                //Stop for loop
             //   validTrack=true;
                cmd_vel_pub.publish(cmd_vel);

    //        ROS_INFO("xLast1: %f", xLast1);
    //        ROS_INFO("yLast1: %f", yLast1);
    //        ROS_INFO("yDirection: %f", yDirection);
            }
        }


/*    else{
        xPath= xRobot+cos(orientationRobot+AngleErrorKinect)*tempDistanceKinect;
        yPath= yRobot+sin(orientationRobot+AngleErrorKinect)*tempDistanceKinect;
        AngleErrorFollow=PI-atan2(yPath-yRobot,(xPath-xRobot))-PI+orientationRobot;
            if(abs(AngleErrorFollow)>PI){
                if(AngleErrorFollow<0){AngleErrorFollow=AngleErrorFollow+2*PI;}
                else{AngleErrorFollow=AngleErrorFollow-2*PI;}
            }
//            ROS_INFO("AngleErrorFollow: %f", AngleErrorFollow);
            tempDistance=sqrt(pow(xPath-xRobot,2)+pow(yPath-yRobot,2))-count*50*0.3/1000;
            count++;
//            ROS_INFO("tempDistance: %f", tempDistance);

            if (!laser_obstacle_flag){
           // tempAngular=atan2(yLast1,xLast1);
            cmd_vel.angular.z =-AngleErrorFollow*KpAngle;
            if(AngleErrorFollow<0.5){cmd_vel.angular.z =0;}
    //       if(AngleErrorFollow<0.05&&tempDistance>0.5){
                double command_speed=0.3;
                cmd_vel.linear.x = command_speed;
          //      cmd_vel_pub.publish(cmd_vel);
            if(tempDistance<0.5){
                cmd_vel.linear.x = 0;
                if (yDirection>0){cmd_vel.angular.z=0.2;}
                else{cmd_vel.angular.z=-0.2;}
            }
//        }
            //Stop for loop
            validTrack=true;
            cmd_vel_pub.publish(cmd_vel);

//        ROS_INFO("xLast1: %f", xLast1);
//        ROS_INFO("yLast1: %f", yLast1);
//        ROS_INFO("yDirection: %f", yDirection);
    }
    }
*/

}
Ejemplo n.º 28
0
void publishRxmSFRB(const ublox_msgs::RxmSFRB& m)
{
  static ros::Publisher publisher = nh->advertise<ublox_msgs::RxmSFRB>("rxmsfrb", kROSQueueSize);
  publisher.publish(m);
}
void TeleopAria::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("Use arrow keys to move the the Robot.");


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

    myTransRatio=0.0, myRotRatio=0.0;
    ROS_DEBUG("value: 0x%02X\n", c);

    switch(c)
    {
      case KEYCODE_L:
        ROS_DEBUG("LEFT");
        myRotRatio = 100;
        dirty = true;
        break;
      case KEYCODE_R:
        ROS_DEBUG("RIGHT");
        myRotRatio = -100;
        dirty = true;
        break;
      case KEYCODE_U:
        ROS_DEBUG("UP");
        myTransRatio = 100;
        dirty = true;
        break;
      case KEYCODE_D:
        ROS_DEBUG("DOWN");
        myTransRatio = -100;
        dirty = true;
        break;
    }


    ariaClientDriver::AriaCommandData msg;
    	msg.TransRatio=myTransRatio;
      	msg.RotRatio=myRotRatio;
      	msg.LatRatio=myLatRatio;
      	msg.MaxVel=myMaxVel;
    if(dirty ==true)
    {
    	myCommandDataPublisher.publish(msg);
      dirty=false;
    }
  }


  return;
}
Ejemplo n.º 30
0
void publishStatusTimerEventHandler(const ros::TimerEvent&)
{
  std_msgs::String msg;
  msg.data = "UNM ALSA";
  status_publisher.publish(msg);
}