Cannot write Command and read Message

MAVLink
1

When I try to send command using mavlink version 2, but it seems the px4 does not receive any data at all.

First of all, I send command by MAV_CMD_SET_MESSAGE_INTERVAL for the message like HIGHRES_IMU, ATTITUDE_QUATERNION.

Secondly, I do not know why after launching mavros, my code can read message (HIGHRES_IMU and ATTITUDE_QUATERNION).

The below is my code.

Both of the variable of sysid_ and compid_ is initialized to 1.

current_messages_.sysid = sysid_;
current_messages_.compid = compid_;

publisher_setup();

port_->start();

usleep(1000);

mavlink_message_t message;

current_messages_.sysid = message.sysid;
current_messages_.compid = message.compid;

printf("System id: %d\n", current_messages_.sysid);
printf("Component id: %d\n", current_messages_.compid);

mavlink_message_t message_to_write;

mavlink_command_long_t com = {0};

com.target_system = message.sysid;
com.target_component = message.compid;
com.confirmation = true;
com.command = MAV_CMD_REQUEST_MESSAGE;
com.param1 = MAVLINK_MSG_ID_HEARTBEAT;
com.param2 = 1000000;

mavlink_msg_command_long_encode(sysid_, compid_, 
&message_to_write, &com);
port->write_message(&message_to_write);

com.param1 = MAVLINK_MSG_ID_HIGHRES_IMU;

mavlink_msg_command_long_encode(sysid_, compid_, 
&message_to_write, &com);
port->write_message(&message_to_write);

com.param1 = MAVLINK_MSG_ID_ATTITUDE_QUATERNION;

mavlink_msg_command_long_encode(sysid_, compid_,
&message_to_write, &com);
port->write_message(&message_to_write);

com.command = MAV_CMD_SET_MESSAGE_INTERVAL;
com.param1 = MAVLINK_MSG_ID_HIGHRES_IMU;
com.param2 = 2500.0;

mavlink_msg_command_long_encode(sysid_, compid_, 
&message_to_write, &com);

port->write_message(&message_to_write);

com.param1 = MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
mavlink_msg_command_long_encode(sysid_, compid_, 
&message_to_write, &com);

port->write_message(&message_to_write);