static GstFlowReturn new_sample_callback (GstAppSink * sink, gpointer user_data)
{
GstBuffer *buffer;
GstSample *sample;
Encoder *encoder = (Encoder *)user_data;
*(encoder->output->heartbeat) = gst_clock_get_time (encoder->system_clock);
sample = gst_app_sink_pull_sample (GST_APP_SINK (sink));
buffer = gst_sample_get_buffer (sample);
sem_wait (encoder->output->semaphore);
(*(encoder->output->total_count)) += gst_buffer_get_size (buffer);
/* update head_addr, free enough memory for current buffer. */
while (cache_free (encoder) < gst_buffer_get_size (buffer) + 12) { /* timestamp + gop size = 12 */
move_head (encoder);
}
if (!GST_BUFFER_FLAG_IS_SET (buffer, GST_BUFFER_FLAG_DELTA_UNIT)) {
/*
* random access point found.
* write previous gop size to 4 bytes reservation,
* write current gop timestamp,
* reserve 4 bytes for size of current gop,
*/
if (encoder->mqdes == -1) {
/* no m3u8 output */
move_last_rap (encoder, buffer);
} else if (GST_BUFFER_PTS (buffer) == encoder->last_running_time) {
gchar *msg;
move_last_rap (encoder, buffer);
msg = g_strdup_printf ("%lu", encoder->last_segment_duration);
if (mq_send (encoder->mqdes, msg, strlen (msg), 1) == -1) {
GST_ERROR ("mq_send error: %s", g_strerror (errno));
}
g_free (msg);
encoder->last_running_time = GST_CLOCK_TIME_NONE;
}
}
/* udpstreaming? */
if (encoder->udpstreaming) {
udp_streaming (encoder, buffer);
}
/*
* copy buffer to cache.
* update tail_addr
*/
copy_buffer (encoder, buffer);
sem_post (encoder->output->semaphore);
gst_sample_unref (sample);
return GST_FLOW_OK;
}
void Snake::move() {
// the position of the last piece before moving
// is the position of the new spawned piece
// if the snake is growing
map_coords grow_position = parts.back();
move_body();
move_head();
grow_part(grow_position);
}
void spin ()
{
ros::Rate loop_rate(rate_);
while (ros::ok())
{
if (cloud_and_image_received_)
{
// swing left to right and right to left by increasing y step
if ( (((move_offset_y_min_ < current_position_y_) && (current_position_y_ < move_offset_y_max_))
|| (fabs(current_position_y_ - move_offset_y_min_) < EPS) || (fabs(current_position_y_ - move_offset_y_max_) < EPS))
&&
((current_position_z_ < move_offset_z_max_)
|| (fabs(current_position_z_ - move_offset_z_min_) < EPS) || (fabs(current_position_z_ - move_offset_z_max_) < EPS)) )
{
current_position_y_ += step_y_;
ROS_INFO("in left to right");
}
// increase z step
else if ( ((current_position_y_ < move_offset_y_min_) || (current_position_y_ > move_offset_y_max_))
&&
(current_position_z_ < move_offset_z_max_) )
{
current_position_z_ += step_z_;
step_y_ = -step_y_;
current_position_y_ += step_y_;
ROS_INFO("in increase z");
}
// check if we are done
else if ( ((current_position_y_ < move_offset_y_min_) || (current_position_y_ > move_offset_y_max_))
&&
((current_position_z_ > move_offset_z_max_) || (fabs(current_position_z_ - move_offset_z_max_) < EPS)) )
{
ROS_INFO("DONE - exiting");
ros::shutdown();
}
else
{
ROS_INFO("Not an option");
}
move_head("base_link", current_position_x_, current_position_y_, current_position_z_);
cloud_and_image_received_ = false;
}
ros::spinOnce();
loop_rate.sleep();
}
}
int snake_move(SNAKE *snake, int *food)
{
// 丢掉最后一节身体
pop_last_body(snake);
// 在头部加个新身体
new_body_affer_head(snake);
// 脑袋前进一下
move_head(snake);
mvaddch(snake->y, snake->x, snake->head_chr);
// 吃豆儿
if (snake->y == food[1] && snake->x == food[0])
{
add_last_body(snake);
food = random_food(snake);
draw_food(food);
if (speed - speed_up > speed_max)
{
speed = speed - speed_up;
}
}
// 死蛇
if ((judge_dead(snake)) == 1)
{
snake->head_chr = 'X';
snake->body_chr = 'x';
init_pair(3, COLOR_RED, COLOR_BLACK);
attron(COLOR_PAIR(3));
draw_whole_snake(snake);
char dead_str[] = "OH, THE POOR LITTLE SNAKE";
mvprintw(LINES/2-1, COLS/2-sizeof(dead_str)/2-1, dead_str );
refresh();
attroff(COLOR_PAIR(3));
return 1;
}
mvprintw(0, 0, "snake head at: %d, %d; snake length: %d; food at: %d, %d; ", snake->x, snake->y, snake->length, food[0], food[1]);
refresh();
return 0;
}
void serviceGame(void) {
move_head(change_dir);
if (ate_fruit(corners[head].x,corners[head].y))
{
snake_length_limit += (snake_length_limit/10);
make_fruit();
}
if (collision()) game_over();
else {
gameSetMetaPixel(corners[head].x, corners[head].y, ON); //Redraw
if (snake_length_current > snake_length_limit)
{
gameSetMetaPixel(corners[tail].x, corners[tail].y, OFF); //Erase
follow_tail();
}
}
move_tick = 0;
}
PointCloudCapturer(ros::NodeHandle &n)
: nh_(n), synchronizer_( MySyncPolicy(1), cloud_sub_, camera_sub_)
{
nh_.param("input_cloud_topic", input_cloud_topic_, std::string("/kinect_head/camera/rgb/points"));
nh_.param("input_image_topic", input_image_topic_, std::string("/kinect_head/camera/rgb/image_color"));
nh_.param("input_camera_info_topic", input_camera_info_topic_, std::string("/kinect_head/camera/rgb/camera_info"));
// cloud_sub_= nh_.subscribe (input_cloud_topic_, 10, &PointCloudCapturer::cloud_cb, this);
camera_sub_.subscribe( nh_, input_image_topic_, 1000 );
cloud_sub_.subscribe( nh_, input_cloud_topic_, 1000 );
sync_connection_ = synchronizer_.registerCallback( &PointCloudCapturer::callback, this );
ROS_INFO("[PointCloudColorizer:] Subscribing to cloud topic %s", input_cloud_topic_.c_str());
point_head_client_ = new PointHeadClient("/head_traj_controller/point_head_action", true);
//wait for head controller action server to come up
while(!point_head_client_->waitForServer(ros::Duration(5.0)) && ros::ok())
{
ROS_INFO("Waiting for the point_head_action server to come up");
}
cloud_and_image_received_ = false;
nh_.param("move_offset_y_min", move_offset_y_min_, -1.5);
nh_.param("move_offset_y_max", move_offset_y_max_, 1.5);
nh_.param("step_y", step_y_, 0.3);
nh_.param("move_offset_z_min", move_offset_z_min_, 0.3);
nh_.param("move_offset_z_max", move_offset_z_max_, 1.5);
nh_.param("step_z", step_z_, 0.3);
nh_.param("move_offset_x", move_offset_x_, 1.0);
nh_.param("bag_name", bag_name_, std::string("bosch_kitchen_tr.bag"));
nh_.param("to_frame", to_frame_, std::string("base_link"));
nh_.param("rate", rate_, 1.0);
current_position_x_ = move_offset_x_;
current_position_y_ = move_offset_y_min_;
current_position_z_ = move_offset_z_min_;
move_head("base_link", current_position_x_, current_position_y_, current_position_z_);
EPS = 1e-5;
//thread ROS spinner
spin_thread_ = boost::thread (boost::bind (&ros::spin));
bag_.open(bag_name_, rosbag::bagmode::Write);
}
static GstFlowReturn new_sample_callback (GstAppSink * sink, gpointer user_data)
{
GstBuffer *buffer;
GstSample *sample;
Encoder *encoder = (Encoder *)user_data;
*(encoder->output->heartbeat) = gst_clock_get_time (encoder->system_clock);
sample = gst_app_sink_pull_sample (GST_APP_SINK (sink));
buffer = gst_sample_get_buffer (sample);
if (sem_wait (encoder->output->semaphore) == -1) {
GST_ERROR ("new_sample_callback sem_wait failure: %s", g_strerror (errno));
gst_sample_unref (sample);
return GST_FLOW_OK;
}
(*(encoder->output->total_count)) += gst_buffer_get_size (buffer);
/* update head_addr, free enough memory for current buffer. */
while (cache_free (encoder) <= gst_buffer_get_size (buffer) + 12) { /* timestamp + gop size = 12 */
move_head (encoder);
}
if (encoder->has_tssegment && encoder->has_m3u8_output) {
if ((encoder->duration_accumulation >= encoder->segment_duration) ||
((encoder->segment_duration - encoder->duration_accumulation) < 500000000)) {
encoder->last_segment_duration = encoder->duration_accumulation;
encoder->last_running_time = GST_BUFFER_PTS (buffer);
encoder->duration_accumulation = 0;
}
encoder->duration_accumulation += GST_BUFFER_DURATION (buffer);
}
/*
* random access point found.
* 1. with video encoder and IDR found;
* 2. audio only encoder and current pts >= last_running_time;
* 3. tssegment out every buffer with random access point.
*/
if ((encoder->has_video && !GST_BUFFER_FLAG_IS_SET (buffer, GST_BUFFER_FLAG_DELTA_UNIT)) ||
(encoder->has_audio_only && (GST_BUFFER_PTS (buffer) >= encoder->last_running_time)) ||
(encoder->has_tssegment && (GST_BUFFER_PTS (buffer) >= encoder->last_running_time))) {
if (encoder->has_m3u8_output == FALSE) {
/* no m3u8 output */
move_last_rap (encoder, buffer);
} else if (GST_BUFFER_PTS (buffer) >= encoder->last_running_time) {
move_last_rap (encoder, buffer);
send_msg (encoder);
} else if (encoder->is_first_key) {
/* move_last_rap if its first key even if has m3u8 output */
move_last_rap (encoder, buffer);
send_msg (encoder);
encoder->is_first_key = FALSE;
}
}
/* udpstreaming? */
if (encoder->udpstreaming) {
udp_streaming (encoder, buffer);
}
/*
* copy buffer to cache.
* update tail_addr
*/
copy_buffer (encoder, buffer);
sem_post (encoder->output->semaphore);
gst_sample_unref (sample);
return GST_FLOW_OK;
}
