C++ Publisher::publish方法代码示例

    //! 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
//.........这里部分代码省略.........

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

//.........这里部分代码省略.........
			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
			/*

/* 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");

}

void publishAidHUI(const ublox_msgs::AidHUI& m)
{
  static ros::Publisher publisher = nh->advertise<ublox_msgs::AidHUI>("aidhui", kROSQueueSize);
  publisher.publish(m);
}

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

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.
//.........这里部分代码省略.........

//.........这里部分代码省略.........
	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;
}

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;
//.........这里部分代码省略.........

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);
//.........这里部分代码省略.........

void SvmDetect::CloudClustersCallback(const lidar_tracker::CloudClusterArray::Ptr& in_cloud_cluster_array_ptr)
{
	cloud_clusters_pub_.publish(*in_cloud_cluster_array_ptr);
}

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);
//.........这里部分代码省略.........

void publishAidALM(const ublox_msgs::AidALM& m)
{
  static ros::Publisher publisher = nh->advertise<ublox_msgs::AidALM>("aidalm", kROSQueueSize);
  publisher.publish(m);
}