int main() { // SET single LED port DDRC |= _BV(LED_PIN); PORTC ^= _BV(LED_PIN); // 1s pull up of LED Strip pins */ apa102_DDRREG &= ~_BV(apa102_data); apa102_DDRREG &= ~_BV(apa102_clk); apa102_PORTREG |= _BV(apa102_data); apa102_PORTREG |= _BV(apa102_clk); _delay_ms(900); // Disable pull ups apa102_PORTREG &= ~_BV(apa102_data); apa102_PORTREG &= ~_BV(apa102_clk); apa102_DDRREG |= _BV(apa102_data); apa102_DDRREG |= _BV(apa102_clk); _delay_ms(100); PORTC ^= _BV(LED_PIN); // Clear LEDs in the strip clear_all_leds(); // Init ROS nh.initNode(); nh.subscribe(set_sub); nh.subscribe(set_led_sub); ack_led(); // Wait for Server side to start while (!nh.connected()) { nh.spinOnce(); // LUFA functions that need to be called frequently to keep USB alive CDC_Device_USBTask(&Atmega32u4Hardware::VirtualSerial_CDC_Interface); USB_USBTask(); _delay_ms(10); } ack_led(); // Publish some debug information snprintf(log_str, MAX_MSG_SIZE, "V:%s", GIT_VERSION); nh.loginfo(log_str); snprintf(log_str, MAX_MSG_SIZE, "FM:%d", get_free_ram()); nh.loginfo(log_str); while(1) { nh.spinOnce(); // LUFA functions that need to be called frequently to keep USB alive CDC_Device_USBTask(&Atmega32u4Hardware::VirtualSerial_CDC_Interface); USB_USBTask(); } return 0; }
void set_cb(const autonomy_leds_msgs::Command& cmd_msg) { switch (cmd_msg.flag) { case autonomy_leds_msgs::Command::FLAG_SET_ALL: { ros_buffer_ptr = cmd_msg.colors_vec; ros_buffer_size = cmd_msg.colors_vec_length; apa102_setleds_packed(ros_buffer_ptr, ros_buffer_size); break; } case autonomy_leds_msgs::Command::FLAG_CLEAR: { // Clear everything, it should work even w/o a buffer clear_all_leds(); ros_buffer_size = 0; break; } case autonomy_leds_msgs::Command::FLAG_SHIFTLEFT: { if (ros_buffer_size == 0) break; uint16_t buffer = ros_buffer_ptr[0]; for (led_counter = 0; led_counter < ros_buffer_size - 1; led_counter++) { ros_buffer_ptr[led_counter] = ros_buffer_ptr[led_counter + 1]; } ros_buffer_ptr[led_counter] = buffer; apa102_setleds_packed(ros_buffer_ptr, ros_buffer_size); break; } case autonomy_leds_msgs::Command::FLAG_SHIFTRIGHT: { if (ros_buffer_size == 0) break; uint16_t buffer = ros_buffer_ptr[ros_buffer_size - 1]; for (led_counter = ros_buffer_size - 1; led_counter > 0; led_counter--) { ros_buffer_ptr[led_counter] = ros_buffer_ptr[led_counter - 1]; } ros_buffer_ptr[led_counter] = buffer; apa102_setleds_packed(ros_buffer_ptr, ros_buffer_size); break; } case autonomy_leds_msgs::Command::FLAG_INVERT: { if (ros_buffer_size == 0) break; for (led_counter = 0; led_counter < ros_buffer_size; led_counter++) { ros_buffer_ptr[led_counter] ^= 0b0111111111111111; } apa102_setleds_packed(ros_buffer_ptr, ros_buffer_size); break; } } #ifdef PUBLISH_FREE_RAM snprintf(log_str, MAX_MSG_SIZE, "%d", get_free_ram()); nh.loginfo(log_str); #endif }
static inline void quaternionTFToMsg(const geometry_msgs::Quaternion& bt, geometry_msgs::Quaternion& msg) { if (fabs(length2(bt) - 1 ) > QUATERNION_TOLERANCE) { nh.loginfo("TF to MSG: Quaternion Not Properly Normalized"); geometry_msgs::Quaternion bt_temp = bt; bt_temp = normalize(bt_temp); msg.x = bt_temp.x; msg.y = bt_temp.y; msg.z = bt_temp.z; msg.w = bt_temp.w; } else { msg.x = bt.x; msg.y = bt.y; msg.z = bt.z; msg.w = bt.w; } };
void callback(const geometry_msgs::PoseArray& msg) { sum_msg.position.x = 0; sum_msg.position.y = 0; sum_msg.position.z = 0; char message[40]; sprintf(message, "%d %d %d %d", msg.poses_length, (int) msg.poses[0].position.x, (int) msg.poses[0].position.y, (int) msg.poses[0].position.z); node_handle.loginfo(message); for(int i = 0; i < msg.poses_length; i++) { sum_msg.position.x += msg.poses[i].position.x; sum_msg.position.y += msg.poses[i].position.y; sum_msg.position.z += msg.poses[i].position.z; } publisher.publish(&sum_msg); }
int main() { nh.initNode(); nh.advertise(chatter); while (1) { str_msg.data = hello; chatter.publish( &str_msg ); nh.logdebug(debug); nh.loginfo(info); nh.logwarn(warn); nh.logerror(errors); nh.logfatal(fatal); nh.spinOnce(); wait_ms(500); } }