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