erratic.cc

机器人仿真软件

	for(;;)
	{
		pthread_testcancel();

		// Wait for some instructions
		Wait();

		this->Lock();

		// we want to reset the odometry and enable the motors if the first
		// client just subscribed to the position device, and we want to stop
		// and disable the motors if the last client unsubscribed.
		if(!last_position_subscrcount && this->position_subscriptions)
		{
			this->ToggleMotorPower(0);
			this->ResetRawPositions();
		}
		else if(last_position_subscrcount && !(this->position_subscriptions))
		{
			// enable motor power
			this->ToggleMotorPower(1);
		}
		last_position_subscrcount = this->position_subscriptions;


		// We'll ask the robot to enable analog packets if we just got our
		// first subscriber
		if(!last_aio_ir_subscriptions && this->aio_ir_subscriptions)
			this->ToggleAIn(1);
		else if(last_aio_ir_subscriptions && !(this->aio_ir_subscriptions))
			this->ToggleAIn(0);
		last_aio_ir_subscriptions = this->aio_ir_subscriptions;


		this->Unlock();

		// handle pending messages
		//printf( "Will look for incoming messages\n" );
		if(!this->InQueue->Empty())
		{
			ProcessMessages();
		}
		else
		{
			// if no pending msg, resend the last position cmd.
			//TODO reenable?
			//this->HandlePositionCommand(this->last_position_cmd);
		}
		
		// Do some little nice printout
		if (print_status_summary) {
			
		}
	}
}

// Publishes the indicated interface data
void Erratic::PublishPosition2D() {
	this->Publish(this->position_id, NULL,
		PLAYER_MSGTYPE_DATA,
		PLAYER_POSITION2D_DATA_STATE,
		(void*)&(this->erratic_data.position),
		sizeof(player_position2d_data_t),
		NULL);
}
void Erratic::PublishPower() {
	this->Publish(this->power_id, NULL,
		PLAYER_MSGTYPE_DATA,
		PLAYER_POWER_DATA_STATE,
		(void*)&(this->erratic_data.power),
		sizeof(player_power_data_t),
		NULL);
}
void Erratic::PublishAIn() {
	this->Publish(this->aio_id, NULL,
		PLAYER_MSGTYPE_DATA,
		PLAYER_AIO_DATA_STATE,
		(void*)&(this->erratic_data.aio),
		sizeof(player_aio_data_t),
		NULL);
}
void Erratic::PublishIR() {
	this->Publish(this->ir_id, NULL,
		PLAYER_MSGTYPE_DATA,
		PLAYER_IR_DATA_RANGES,
		(void*)&(this->erratic_data.ir),
		sizeof(player_ir_data_t),
		NULL);
}
void Erratic::PublishAllData() {
	this->PublishPosition2D();
	this->PublishPower();
	this->PublishAIn();
	this->PublishIR();
}

// Gets called from ProcessMessages to handle one message
int Erratic::ProcessMessage(MessageQueue * resp_queue, player_msghdr * hdr, void * data) {
	// Look for configuration requests
	if(hdr->type == PLAYER_MSGTYPE_REQ)
		return(this->HandleConfig(resp_queue,hdr,data));
	else if(hdr->type == PLAYER_MSGTYPE_CMD)
		return(this->HandleCommand(hdr,data));
	else
		return(-1);
}

// Handles one Player command with a configuration request
int Erratic::HandleConfig(MessageQueue* resp_queue, player_msghdr * hdr, void * data) {

	// check for position config requests
	if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
	                             PLAYER_POSITION2D_REQ_SET_ODOM,
	                             this->position_id))
	{
		if(hdr->size != sizeof(player_position2d_set_odom_req_t))
		{
			PLAYER_WARN("Arg to odometry set requests wrong size; ignoring");
			return(-1);
		}
		player_position2d_set_odom_req_t* set_odom_req =
			(player_position2d_set_odom_req_t*)data;

		this->motor_packet->x_offset = ((int)rint(set_odom_req->pose.px*1e3))
			                           - this->motor_packet->xpos;
		this->motor_packet->y_offset = ((int)rint(set_odom_req->pose.py*1e3))
		                             - this->motor_packet->ypos;
		this->motor_packet->angle_offset = ((int)rint(RTOD(set_odom_req->pose.pa)))
		                                 - this->motor_packet->angle;

		this->Publish(this->position_id, resp_queue,
		              PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_SET_ODOM);
		return(0);
	}
	else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
	                                  PLAYER_POSITION2D_REQ_MOTOR_POWER,
	                                  this->position_id)) {
		if(hdr->size != sizeof(player_position2d_power_config_t)) {
			PLAYER_WARN("Arg to motor state change request wrong size; ignoring");
			return(-1);
		}
		player_position2d_power_config_t* power_config =
			(player_position2d_power_config_t*)data;
		this->ToggleMotorPower(power_config->state);

		this->Publish(this->position_id, resp_queue,
		              PLAYER_MSGTYPE_RESP_ACK,
		              PLAYER_POSITION2D_REQ_MOTOR_POWER);
		return(0);
	} else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
	                                    PLAYER_POSITION2D_REQ_RESET_ODOM,
	                                    this->position_id)) {
		if(hdr->size != 0) {
			PLAYER_WARN("Arg to reset position request is wrong size; ignoring");
			return(-1);
		}

		ResetRawPositions();

		this->Publish(this->position_id, resp_queue,
			PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_RESET_ODOM);
		return(0);
	}
	else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
	                                  PLAYER_POSITION2D_REQ_GET_GEOM,
	                                  this->position_id)) {
		/* Return the robot geometry. */
		if(hdr->size != 0) {
			PLAYER_WARN("Arg get robot geom is wrong size; ignoring");
			return(-1);
		}
		player_position2d_geom_t geom;

		geom.pose.px = -RobotParams[param_idx]->RobotAxleOffset / 1e3;
		geom.pose.py = 0.0;
		geom.pose.pa = 0.0;

		geom.size.sl = RobotParams[param_idx]->RobotLength / 1e3;
		geom.size.sw = RobotParams[param_idx]->RobotWidth / 1e3;

		this->Publish(this->position_id, resp_queue,
		              PLAYER_MSGTYPE_RESP_ACK,
		              PLAYER_POSITION2D_REQ_GET_GEOM,
		              (void*)&geom, sizeof(geom), NULL);
		return(0);
	}
	else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
	                                  PLAYER_POSITION2D_REQ_VELOCITY_MODE,
	                                  this->position_id)) {
		/* velocity control mode:
		 *   0 = direct wheel velocity control
		 *   1 = separate translational and rotational control
		 */
			if(hdr->size != sizeof(player_position2d_velocity_mode_config_t))
		{
			PLAYER_WARN("Arg to velocity control mode change request is wrong "
				"size; ignoring");
			return(-1);
		}
		player_position2d_velocity_mode_config_t* velmode_config =
			(player_position2d_velocity_mode_config_t*)data;

		if(velmode_config->value)
			direct_wheel_vel_control = false;
		else
			direct_wheel_vel_control = true;

		this->Publish(this->position_id, resp_queue,
		              PLAYER_MSGTYPE_RESP_ACK,
		              PLAYER_POSITION2D_REQ_VELOCITY_MODE);
		return(0);
	}
	
	// Request for getting IR locations
	else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
	                                  PLAYER_IR_POSE,
	                                  this->ir_id)) {
		player_ir_pose_t pose;
		pose.poses_count = RobotParams[param_idx]->NumIR;
		for (uint16_t i = 0; i < pose.poses_count ;i++)
			pose.poses[i] = RobotParams[param_idx]->IRPose[i];
		
		this->Publish(this->ir_id, resp_queue,
		              PLAYER_MSGTYPE_RESP_ACK,
		              PLAYER_IR_POSE,
		              (void*)&pose, sizeof(pose), NULL);
		return(0);
	}
	else
	{
		PLAYER_WARN("unknown config request to erratic driver");
		return(-1);
	}
}

// Process car-like command, which sets an angular position target and
// translational velocity target.
// Erratic handles this natively
//
void 
Erratic::HandleCarCommand(player_position2d_cmd_car_t cmd)
{
	int speedDemand, angleDemand;
	unsigned short absspeedDemand, absangleDemand;
	unsigned char motorcommand[4];
	ErraticPacket *motorpacket;

	speedDemand = (int)rint(cmd.velocity * 1e3);	// convert to mm/s
	angleDemand = (int)rint(RTOD(cmd.angle)); // convert to deg heading
	angleDemand -= this->motor_packet->angle_offset;	// check for angle offset of odometry
	if (angleDemand > 360)				// normalize
		angleDemand -= 360;
	if (angleDemand < 0)
		angleDemand += 360;

	// do separate trans and rot vels
	motorcommand[0] = (command_e)trans_vel;
	if(speedDemand >= 0)
		motorcommand[1] = (argtype_e)argint;
	else
		motorcommand[1] = (argtype_e)argnint;

	absspeedDemand = (unsigned short)abs(speedDemand);
	if(absspeedDemand < this->motor_max_speed)
		{
			motorcommand[2] = absspeedDemand & 0x00FF;
			motorcommand[3] = (absspeedDemand & 0xFF00) >> 8;
		}
	else
		{
			motorcommand[2] = this->motor_max_speed & 0x00FF;
			motorcommand[3] = (this->motor_max_speed & 0xFF00) >> 8;
		}
		
		motorpacket = new ErraticPacket();
		motorpacket->Build(motorcommand, 4);
		Send(motorpacket);


	// do separate trans and rot vels
	motorcommand[0] = (command_e)rot_pos;
	if(angleDemand >= 0)
		motorcommand[1] = (argtype_e)argint;
	else
		motorcommand[1] = (argtype_e)argnint;

	absangleDemand = (unsigned short)abs(angleDemand);
	motorcommand[2] = absangleDemand & 0x00FF;
	motorcommand[3] = (absangleDemand & 0xFF00) >> 8;
		
	motorpacket = new ErraticPacket();
	motorpacket->Build(motorcommand, 4);
	Send(motorpacket);
}



// Handles one Player command detailing a velocity
void Erratic::HandlePositionCommand(player_position2d_cmd_vel_t position_cmd) {
	int speedDemand, turnRateDemand;
	double leftvel, rightvel;
	double rotational_term;
	unsigned short absspeedDemand, absturnRateDemand;
	unsigned char motorcommand[4];
	ErraticPacket *motorpacket;

	speedDemand = (int)rint(position_cmd.vel.px * 1e3);
	turnRateDemand = (int)rint(RTOD(position_cmd.vel.pa));

	//speedDemand = 0;
	//turnRateDemand = 768;

	//if (debug_mode)
	//	printf("Will VW, %i and %i\n", speedDemand, turnRateDemand);

	if(this->direct_wheel_vel_control)
	{
		// convert xspeed and yawspeed into wheelspeeds
		rotational_term = (M_PI/180.0) * turnRateDemand /
			RobotParams[param_idx]->DiffConvFactor;

		//printf("rotational_term: %g\n", rotational_term);

		leftvel = (speedDemand - rotational_term);
		rightvel = (speedDemand + rotational_term);

		//printf("Speeds in ticks: %g %g\n", leftvel, rightvel);

		// Apply wheel speed bounds
		if(fabs(leftvel) > this->motor_max_speed)
		{
			if(leftvel > 0)
			{
				leftvel = this->motor_max_speed;
				rightvel *= this->motor_max_speed/leftvel;
			}
			else
			{
				leftvel = -this->motor_max_speed;
				rightvel *= -this->motor_max_speed/leftvel;
			}
		}
		if(fabs(rightvel) > this->motor_max_speed)
		{
			if(rightvel > 0)
			{
				rightvel = this->motor_max_speed;
				leftvel *= this->motor_max_speed/rightvel;
			}
			else
			{
				rightvel = -this->motor_max_speed;
				leftvel *= -this->motor_max_speed/rightvel;
			}
		}

		// Apply control band bounds
		if(this->use_vel_band)
		{
			// This band prevents the wheels from turning in opposite
			// directions
			//TODO WHY is this here??
			// It's disabled elsewhere now.
			if (leftvel * rightvel < 0)
			{
				if (leftvel + rightvel >= 0)
				{
					if (leftvel < 0)
						leftvel = 0;
					if (rightvel < 0)
						rightvel = 0;
				}
				else
				{
					if (leftvel > 0)
						leftvel = 0;
					if (rightvel > 0)
						rightvel = 0;
				}
			}
		}

		// Apply byte range bounds
		if (leftvel / RobotParams[param_idx]->Vel2Divisor > 126)
			leftvel = 126 * RobotParams[param_idx]->Vel2Divisor;
		if (leftvel / RobotParams[param_idx]->Vel2Divisor < -126)
			leftvel = -126 * RobotParams[param_idx]->Vel2Divisor;
		if (rightvel / RobotParams[param_idx]->Vel2Divisor > 126)
			rightvel = 126 * RobotParams[param_idx]->Vel2Divisor;
		if (rightvel / RobotParams[param_idx]->Vel2Divisor < -126)
			rightvel = -126 * RobotParams[param_idx]->Vel2Divisor;

		// send the speed command
		motorcommand[0] = (command_e)wheel_vel;
		motorcommand[1] = (argtype_e)argint;
		motorcommand[2] = (char)(rightvel /
			RobotParams[param_idx]->Vel2Divisor);
		motorcommand[3] = (char)(leftvel /
			RobotParams[param_idx]->Vel2Divisor);

		motorpacket = new ErraticPacket();
		motorpacket->Build(motorcommand, 4);
		Send(motorpacket);
	}
	else
	{
		// do separate trans and rot vels
		motorcommand[0] = (command_e)trans_vel;
		if(speedDemand >= 0)
			motorcommand[1] = (argtype_e)argint;
		else
			motorcommand[1] = (argtype_e)argnint;

		absspeedDemand = (unsigned short)abs(speedDemand);
		if(absspeedDemand < this->motor_max_speed)
		{
			motorcommand[2] = absspeedDemand & 0x00FF;
			motorcommand[3] = (absspeedDemand & 0xFF00) >> 8;
		}
		else
		{
			motorcommand[2] = this->motor_max_speed & 0x00FF;
			motorcommand[3] = (this->motor_max_speed & 0xFF00) >> 8;
		}
		
		motorpacket = new ErraticPacket();
		motorpacket->Build(motorcommand, 4);
		Send(motorpacket);

		motorcommand[0] = (command_e)rot_vel;
		if(turnRateDemand >= 0)
			motorcommand[1] = (argtype_e)argint;
		else
			motorcommand[1] = (argtype_e)argnint;

		absturnRateDemand = (unsigned short)abs(turnRateDemand);
		if(absturnRateDemand < this->motor_max_turnspeed)
		{
			motorcommand[2] = absturnRateDemand & 0x00FF;
			motorcommand[3] = (absturnRateDemand & 0xFF00) >> 8;
		}
		else
		{
			motorcommand[2] = this->motor_max_turnspeed & 0x00FF;
			motorcommand[3] = (this->motor_max_turnspeed & 0xFF00) >> 8;
		}

		motorpacket = new ErraticPacket();
		motorpacket->Build(motorcommand, 4);
		Send(motorpacket);

	}
}

// Switchboard for robot commands, called from ProcessMessage
int Erratic::HandleCommand(player_msghdr * hdr, void* data) {
	if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL, this->position_id))
	{
		player_position2d_cmd_vel_t position_cmd = *(player_position2d_cmd_vel_t*)data;
		this->HandlePositionCommand(position_cmd);
	} else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_CAR, this->position_id))
	{
		player_position2d_cmd_car_t position_cmd = *(player_position2d_cmd_car_t*)data;
		this->HandleCarCommand(position_cmd);
	} else

		return(-1);

	return(0);
}