Esempio n. 1
0
//---------------------------------------------------------------------------
// TESTS:
//---------------------------------------------------------------------------
TEST( CatClass, Serialize_tests )
{
	// initial data
	Cat  data(2,3,4,5,6,7,8);

	// Check the data before the save
	CHECK( data.getX() == 2 );
	CHECK( data.getA() == 3 );
	CHECK( data.getB() == 4 );
	CHECK( data.getY() == 5 );
	CHECK( data.getZ() == 6 );
	CHECK( data.getC() == 7 );
	CHECK( data.getD() == 8 );

	// create a local buffer
	char buff[ 1024 ];

	// serialize the data
	data.serialize( buff );

	// Create a new
	Cat newData;

	// deserialize the data
	newData.deserialize( buff );

	// validate that data is the same
	CHECK( newData.getX() == 2 );
	CHECK( newData.getA() == 3 );
	CHECK( newData.getB() == 4 );
	CHECK( newData.getY() == 5 );
	CHECK( newData.getZ() == 6 );
	CHECK( newData.getC() == 7 );
	CHECK( newData.getD() == 8 );

	// check the size of the data structure
	CHECK( sizeof(Cat) == 24 );
}
Esempio n. 2
0
int main(int argc, char **argv) 
{
	// Initialize the cat robot with the correct position position

	ros::init(argc,argv,"Animal");
	std::string xPosArg = argv[1];
	std::string spacingArg = argv[2];
	double spacing = atof(spacingArg.c_str());
	double xPos = atof(xPosArg.c_str());
	cat = Cat(xPos, 21.500);

	// Create ros handler for this node
	ros::NodeHandle n;

	cat.robotNode_stage_pub = n.advertise<geometry_msgs::Twist>("cmd_vel",1000);

	cat.stageOdo_Sub = n.subscribe<nav_msgs::Odometry>("base_pose_ground_truth",1000,stage_callback);
	ros::Rate loop_rate(10); 

	//subscribe to laser for cats
	cat.baseScan_Sub = n.subscribe<sensor_msgs::LaserScan>("base_scan", 1000,callBackLaserScan);

	// Broadcast the node's status information for other to subscribe to.
	ros::Publisher pub=n.advertise<se306project::animal_status>("status",1000);
	se306project::animal_status status_msg;
	// 0 rotatiing to the side, 1 moving horizontally, 2 rotating to bnorth/south, 3 moving vertical, 4 stop
	while (ros::ok())
	{
		// Message to stage 
		if(cat.getObstacleStatus().compare("Obstacle nearby")!=0){
			cat.move();
		}
		// Give cat a small initial movement to fill in its GUI status
		if (initial) {
			cat.addMovement("forward_x",0.1,1);
		}
		initial = false;

		// Generate random number of seconds for cat to sleep between 50-100s
		double num = (rand() % 100) / 2;
		double time = 50 + num;

		if (cat.getMovementQueueSize() == 0) {
			cat.addMovement("forward_x", 0.1, (0.1 / time));
			cat.faceEast(1);
			cat.addMovement("forward_x",5,1);
			cat.faceWest(1);
			cat.addMovement("forward_x",-5,1);
		}

		// Add Cat variables to status message to be broadcast
		status_msg.status=status;
		status_msg.pos_x=cat.getX();
		status_msg.pos_y=cat.getY();
		status_msg.pos_theta=cat.getAng();
		status_msg.obstacle=cat.getObstacleStatus();
		// Publish status message
		pub.publish(status_msg);
		ros::spinOnce();
		loop_rate.sleep();
		// Determine status of cat
		cat.determineStatus();
		//if ( cat.getLin() == 0.01 ) {
		//	status = "Sleeping";
		//}
	}
	return 0;
}