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

void handleRecievedData(char recv_buf[],Publisher config_pub,Publisher arm_pub)
{
    std_msgs::String msg;
    char type[4]= {'\0'};
    strncpy(type,recv_buf,3);
    if(!strcmp(type,"CAM"))
    {
        int written;
        char temp[20]= {'\0'};
        strncpy(temp,recv_buf+4,strlen(recv_buf));
        ROS_INFO("sent data : %s",temp);
        msg.data = temp;
        config_pub.publish(msg);
    }
    else if(!strcmp(type,"ARM"))
    {
        int written = strlen(recv_buf);
        char temp[20]= {'\0'};
        strncpy(temp,recv_buf+4,written);
        ROS_INFO("sent data : %s",temp);
        msg.data = temp;
        arm_pub.publish(msg);
    }
    else if(!strcmp(type,"PTZ"))
    {
        int written = strlen(recv_buf);
        char temp[20]= {'\0'};
        strncpy(temp,recv_buf+4,written);
        ROS_INFO("sent data : %s",temp);
        msg.data = temp;
        ptz_pub.publish(msg);
    }
}

void sensorPacketCallback(vector<float>& joints_pos, vector<float>& joints_adc, float pressure)
{
	// Send joint state message
	JointState msg_joints;

	msg_joints.header.stamp = Time::now();

	for(int i = 0; i < joints_adc.size(); i++)
	{
		msg_joints.name.push_back(joint_names[i]);
		msg_joints.position.push_back(joints_adc[i]);
	}

	pub_joints.publish(msg_joints);

	// Send pressure message
	Float32 msg_pressure;
	msg_pressure.data = pressure;
	pub_pressure.publish(msg_pressure);

	// Store current positions for IK computations
	for(int i = 0; i < joints_pos.size(); i++)
	{
		q_current(i) = joints_pos[i];
	}
}

void publishStatus(TimerEvent timerEvent) {
	std_msgs::Int32 intMessage;

	intMessage.data = _currentInput + 1;
	_currentInputPublisher.publish(intMessage);

	intMessage.data = _currentOutput + 1;
	_currentOutputPublisher.publish(intMessage);
}

int main (int argc, char* argv[]) {
	gengetopt_args_info args;
	cmdline_parser(argc,argv,&args);
	double v = args.linearVelocity_arg;
	double a = args.angularVelocity_arg;
	double time = args.time_arg;
	int count = 0;
	init(argc, argv, "talker");
	geometry_msgs::Twist msg;
	msg.linear.x = v;
	msg.angular.z = a;

	NodeHandle n;
	Publisher pub = n.advertise<geometry_msgs::Twist>("/cmd_vel_mux/input/navi", 1);
	ros::Rate loop_rate(10);
	while (ros::ok() && count < (int)time*10) {
		ros::spinOnce();
		pub.publish(msg);
		loop_rate.sleep();
		count++;
	}
	for (int i = 0; i < 5; i++) {
		msg.linear.x = 0;
		msg.angular.z = 0;
		pub.publish(msg);
		ros::spinOnce();
	}



	return 0;

	//init(argc, argv, "talker");
	//geometry_msgs::Twist msg;
	//msg.linear.x = 1.0;
	//msg.angular.z = .5;

	//int max_count = 30;
	//int count = 0;
	//NodeHandle n;
	//Publisher pub = n.advertise<geometry_msgs::Twist>("/cmd_vel_mux/input/navi", 1);
	//ros::Rate loop_rate(10);
	//while (ros::ok() && count < max_count) {
	//	ros::spinOnce();
	//	pub.publish(msg);
	//	loop_rate.sleep();
	//	count++;
	//}
	//return 0;
}

int main(int argc, char *argv[])
{
   init(argc, argv, "position");

   int key = 0;
   NodeHandle n;
   Position_Node::Position pos;
   Rate loop_rate(1);     /* in Hz */

   
   Subscriber sub = n.subscribe("MarkerInfo", 1000, markerCallback);
   Publisher pub = n.advertise<Position_Node::Position>("Position", 1000);

   while(ros::ok())
   {
      /* Publish our position */
      pos.x = g_robot_x;
      pos.y = g_robot_y;
      pos.theta = g_robot_theta;

      //ROS_INFO("Published position.\n");
      pub.publish(pos);

      spinOnce();

      loop_rate.sleep();
   }

   return 0;
}

void callback(const StereoPairIMUConstPtr& pair)
{
    ImagePtr left(new Image), right(new Image);
    PoseStampedPtr pose(new PoseStamped);

    *left = pair->pair.leftImage;
    *right = pair->pair.rightImage;
    /**pose = pair->pose.quartenionPose;*/
    *pose = pair->pose.eulerPose;

    leftImagePub.publish(left);
    rightImagePub.publish(right);
    imuPosePub.publish(pose);

    fpsp.print();
}

 Benchmark()
     : nh_()
     , benchmark_state_sub_ (nh_.subscribe ("/roah_rsbb/benchmark/state", 1, &Benchmark::benchmark_state_callback, this))
     , messages_saved_pub_ (nh_.advertise<std_msgs::UInt32> ("/roah_rsbb/messages_saved", 1, true))
 {
     // This should reflect the real number or size of messages saved.
     std_msgs::UInt32 messages_saved_msg;
     messages_saved_msg.data = 1;
     messages_saved_pub_.publish (messages_saved_msg);
 }

int main(int argc, char** argv){
  ros::init(argc, argv, "command_module");

  NodeHandle handler;
  Subscriber s = handler.subscribe("command_message", 1000, messageHandle);
  Subscriber q = handler.subscribe("odom", 1000, odomReceived);
  Publisher p = handler.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
  ROS_INFO("** Command Moudule: On-line");

  ROS_INFO("** Command Module: Waiting for Valid Commands");

  Goal goal;
  ros::Rate loop_rate(10);
  while (ros::ok()) {

	if (!GoalFifo.empty()) {
		goal = GoalFifo.front();

		ROS_INFO("** Command Module: Sending Goal");
		if (goal.dist > 1.0) {
			p.publish(move(0));
			ros::Duration(goal.dist).sleep();
			p.publish(stop());
		}

		if (goal.dTheta > 1.0) {
			p.publish(turn(goal.dir));
			ros::Duration(goal.dTheta).sleep();
			p.publish(stop());
		}	


		p.publish(stop());
                GoalFifo.pop();
	}

	ros::spinOnce();
	loop_rate.sleep();
  }
	

  return 0;
}

/**
 * Controls all the inner workings of the PID functionality
 * Should be called by _timer
 *
 * Controls the heating element Relays manually, overriding the standard relay
 * functionality
 *
 * The pid is designed to Output an analog value, but the relay can only be On/Off.
 *
 * "time proportioning control"  it's essentially a really slow version of PWM.
 * first we decide on a window size. Then set the pid to adjust its output between 0 and that window size.
 * lastly, we add some logic that translates the PID output into "Relay On Time" with the remainder of the
 * window being "Relay Off Time"
 *
 * PID Adaptive Tuning
 * You can change the tuning parameters.  this can be
 * helpful if we want the controller to be agressive at some
 * times, and conservative at others.
 *
 */
void Ohmbrewer::Thermostat::doPID(){
    getSensor()->work();

    setPoint = getTargetTemp()->c();        //targetTemp
    input = getSensor()->getTemp()->c();//currentTemp
    double gap = abs(setPoint-input);   //distance away from target temp
    //SET TUNING PARAMETERS
    if (gap<10) {  //we're close to targetTemp, use conservative tuning parameters
        _thermPID->SetTunings(cons.kP(), cons.kI(), cons.kD());
    }else {//we're far from targetTemp, use aggressive tuning parameters
        _thermPID->SetTunings(agg.kP(), agg.kI(), agg.kD());
    }
    //COMPUTATIONS
    _thermPID->Compute();
    if (millis() - windowStartTime>windowSize) { //time to shift the Relay Window
        windowStartTime += windowSize;
    }
    //TURN ON
    if (getState() && gap!=0) {//if we want to turn on the element (thermostat is ON)
        //TURN ON state and powerPin
        if (!(getElement()->getState())) {//if heating element is off
            getElement()->setState(true);//turn it on
            if (getElement()->getPowerPin() != -1) { // if powerPin enabled
                digitalWrite(getElement()->getPowerPin(), HIGH); //turn it on (only once each time you switch state)
            }
        }
        //RELAY MODULATION
        if (output < millis() - windowStartTime) {
            digitalWrite(getElement()->getControlPin(), HIGH);
        } else {
            digitalWrite(getElement()->getControlPin(), LOW);
        }
    }
    //TURN OFF
    if (gap == 0 || getTargetTemp()->c() <= getSensor()->getTemp()->c() ) {//once reached target temp
        getElement()->setState(false); //turn off element
        getElement()->work();
//        if (getElement()->getPowerPin() != -1) { // if powerPin enabled
//            digitalWrite(getElement()->getPowerPin(), LOW); //turn it off too TODO check this
//        }

        // Notify Ohmbrewer that the target temperature has been reached.
        Publisher pub = Publisher(new String(getStream()),
                                  String("msg"),
                                  String("Target Temperature Reached."));
        pub.add(String("last_read_time"),
                String(getSensor()->getLastReadTime()));
        pub.add(String("temperature"),
                String(getSensor()->getTemp()->c()));
        pub.publish();
    }
}

void transform_cloud(sensor_msgs::PointCloud2 cloud_in) 
{
  bool can_transform = listener.waitForTransform(cloud_in.header.frame_id, 
						 frame_new,
						 ros::Time(0), 
						 ros::Duration(3.0));
  if (can_transform)
    {
      sensor_msgs::PointCloud2 cloud_out;
      cloud_in.header.stamp = ros::Time(0);
      pcl_ros::transformPointCloud(frame_new, cloud_in, cloud_out, listener);
      pub.publish (cloud_out);
    }
}

void publishGUI()
{ 
  static double tLast = 0;
  static double publishInterval = 0.016;
  if(debugLevel<0 || GetTimeSec()-tLast<publishInterval)
    return;
  tLast = GetTimeSec();
  ClearGUI();
  //DrawMap();
  guiMsg.robotLocX = curLoc.x;
  guiMsg.robotLocY = curLoc.y;
  guiMsg.robotAngle = curAngle;
  localization->drawDisplay(guiMsg.lines_p1x, guiMsg.lines_p1y, guiMsg.lines_p2x, guiMsg.lines_p2y, guiMsg.lines_col, guiMsg.points_x, guiMsg.points_y, guiMsg.points_col, guiMsg.circles_x, guiMsg.circles_y, guiMsg.circles_col, 1.0);
  //drawPointCloud();
  guiPublisher.publish(guiMsg);
}

    void operator()(const TimerEvent& t)
    {
        auv.updateAuv (storeforce);
        geometry_msgs::Pose pos_msg;
        kraken_msgs::imuData imu;
        /*************************************/
        pos_msg.position.x=auv._current_position_to_world[0];
        pos_msg.position.y=auv._current_position_to_world[1];
        pos_msg.position.z=auv._current_position_to_world[2];
        //tf::Quaternion quat = tf::createQuaternionFromRPY(auv._current_position_to_world[3],auv._current_position_to_world[4],auv._current_position_to_world[5]);
        tf::Quaternion quat = tf::createQuaternionFromRPY(auv._current_position_to_body[3],auv._current_position_to_body[4],auv._current_position_to_body[5]);
        //tf::Quaternion quat = tf::createQuaternionFromRPY(/*auv._current_position_to_body[3]*/0,/*auv._current_position_to_body[4]*/0,/*auv._current_position_to_body[5]*/0.78);
//        pos_msg.orientation= quat;
        pos_msg.orientation.x=quat.getX ();
        pos_msg.orientation.y=quat.getY ();
        pos_msg.orientation.z=quat.getZ ();
        pos_msg.orientation.w=quat.getW ();
        /*************************************/
        geometry_msgs::TwistStamped twist_msg;
        twist_msg.twist.linear.x=auv._current_velocity_state_to_body[0];
        twist_msg.twist.linear.y=auv._current_velocity_state_to_body[1];
        twist_msg.twist.linear.z=auv._current_velocity_state_to_body[2];
        imu.data[kraken_sensors::gyroX] = twist_msg.twist.angular.x=auv._current_velocity_state_to_body[3];
        imu.data[kraken_sensors::gyroY]= twist_msg.twist.angular.y=auv._current_velocity_state_to_body[4];
        imu.data[kraken_sensors::gyroZ]  = twist_msg.twist.angular.z=auv._current_velocity_state_to_body[5];
        twist_msg.header.frame_id='1';
        /*************************************/
        nav_msgs::Odometry odo_msg;
        odo_msg.pose.pose=pos_msg;
        odo_msg.twist.twist=twist_msg.twist;
        /*************************************/
        sensor_msgs::Imu imu_msg;
        imu_msg.angular_velocity=twist_msg.twist.angular;
        imu.data[kraken_sensors::accelX] = imu_msg.linear_acceleration.x=auv._current_accelaration_to_body[0];
        imu.data[kraken_sensors::accelY] = imu_msg.linear_acceleration.y=auv._current_accelaration_to_body[1];
        imu.data[kraken_sensors::accelZ] = imu_msg.linear_acceleration.z=auv._current_accelaration_to_body[2];
        imu_msg.orientation=pos_msg.orientation;
        //imu_pub.publish(imu_msg);
        imu.data[kraken_sensors::roll] = auv._current_position_to_body[3];
        imu.data[kraken_sensors::pitch] = auv._current_position_to_body[4];
        imu.data[kraken_sensors::yaw] = auv._current_position_to_body[5];
        imu_pub.publish(imu);
        pose_pub.publish(pos_msg);
        if(isLineNeeded)
            odo_pub.publish(odo_msg);
    }

void velCallback(const geometry_msgs::Twist::ConstPtr& msg) {
		geometry_msgs::Twist twist;
	
		if (msg->linear.x < 0)
				twist.linear.x = 0;		
		
		else
				twist.linear.x = new_vel*msg->linear.x;

    
		twist.linear.y = msg->linear.y;
		twist.linear.z = msg->linear.z;
		twist.angular.x = msg->angular.x; 
		twist.angular.y = msg->angular.y; 
		twist.angular.z = msg->angular.z;
		
		vel_pub.publish(twist);
}

int main( int argc, char* argv[])
{
	init( argc, argv, "talker");

	gengetopt_args_info args;
	cmdline_parser( argc, argv, &args);

	double v = args.linearVelocity_arg;

	NodeHandle n;
	Publisher pub = n.advertise<std_msgs::Empty>("/mobile_base/commands/reset_odometry", 1000);

	//kobuki_msgs::Sound msg;
	geometry_msgs::Twist msg;

	//Publisher pub = n.advertise<msg_folder::msg_type>("topic name",1);


	msg.linear.x =0;

	double t = 10.0;

	ros::Rate loop_rate(10);

	int count = 0;
	int max_count = t*10;

	while( ros::ok() && count < max_count )
	{
		msg.angular.z = 1*sin(count/10.0);
		pub.publish(msg);
		ros:spinOnce();
		loop_rate.sleep() ;
		count++;

	}

}

void pid_update() {

    /* PID position update */
    ctrl_data[0] = pid_vision_x -> update(img_center[1], dt); // OpenCV's x-axis is drone's y-axis
    ctrl_data[1] = pid_vision_y -> update(img_center[0], dt);
    //ctrl_data[2] = pid_z -> update(current_position.z, dt);
    ctrl_data[3] = 0; //yaw

    /* Control speed limiting */
    window(&ctrl_data[0], visionLimit);
    window(&ctrl_data[1], visionLimit);
    window(&ctrl_data[2], visionLimit);

    /* Publish the output control velocity from PID controller */
    geometry_msgs::Vector3 velocity;

    velocity.x = ctrl_data[0];
    velocity.y = ctrl_data[1];
    velocity.z = ctrl_data[2];

    ctrl_vel_pub.publish(velocity);


}