BOOL APIENTRY DllMain(HMODULE /*module*/, DWORD reason_for_call, LPVOID /*reseved*/)
{
if (reason_for_call == DLL_PROCESS_DETACH)
{
serial_port.close();
}
return TRUE;
}
SCSAPI_VOID scs_telemetry_shutdown(void)
{
send_empty_packet();
serial_port.close();
game_log(SCS_LOG_TYPE_message, "Plugin shutdown");
game_log = NULL;
}
int main(int argc, char* argv[])
{
printf("[i] Start... \n");
Serial serial;
#if defined(WIN32)
serial.open(SERIAL_PORT_NAME, SERIAL_PORT_RATE, true);
#elif defined(LINUX)
serial.open(SERIAL_PORT_NAME, SERIAL_PORT_RATE);
#endif
if(!serial.connected())
{
printf("[!] Cant open port!\n");
return -1;
}
char c = 'y';
int res = 12;
char buf[BUF_SIZE1];
memset(buf, 0, BUF_SIZE1);
while(true)
{
serial.write( &c, 1 );
#if 0
if(res = serial.available()){
if( res = serial.Read(buf, res) ){
printf("%d %s\n", res, buf);
}
}
#else
if(serial.waitInput(1000)==0)
printf("[i] timeout!\n");
else
{
if(res = serial.available())
{
res = serial.read(buf, res);
printf("%d %s\n", res, buf);
}
}
#endif
}
serial.close();
printf("[i] End.\n");
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "robot_4wd_node");
ros::NodeHandle n;
drive_telemetry_pub = n.advertise<ros_4wd_driver::drive_telemetry_4wd>("drive_telemetry_4wd", 50);
sensors_telemetry_pub = n.advertise<ros_4wd_driver::sensors_telemetry_4wd>("sensors_telemetry_4wd", 50);
imu_raw_data_pub = n.advertise<ros_4wd_driver::imu_raw_data>("imu_raw_data", 50);
motor_service = n.advertiseService("motor_write", motor_write);
odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
cmd_vel_sub = n.subscribe<geometry_msgs::Twist>("cmd_vel", 1, cmd_vel_received);
ROS_INFO("Start");
ROS_INFO("Load params");
if (n.hasParam("/robot_4wd_node/drive_port")) {
ROS_INFO("Load param: drive_port");
n.getParam("/robot_4wd_node/drive_port", drive_serial_name);
}
if (n.hasParam("/robot_4wd_node/sensor_port")) {
ROS_INFO("Load param: sensor_port");
n.getParam("/robot_4wd_node/sensor_port", sensors_serial_name);
}
if (n.hasParam("/robot_4wd_node/baud")) {
ROS_INFO("Load param: baud");
n.getParam("/robot_4wd_node/baud", serial_rate);
}
n.param("/robot_4wd_node/wheel_diameter", wheel_diameter, wheel_diameter);
n.param("/robot_4wd_node/wheel_track", wheel_track, wheel_track);
n.param("/robot_4wd_node/gear_reduction", gear_reduction, gear_reduction);
n.param("/robot_4wd_node/encoder_resolution", encoder_resolution, encoder_resolution);
ticks_per_meter = encoder_resolution * gear_reduction / (wheel_diameter * M_PI);
ROS_INFO("drive_port: %s", drive_serial_name.c_str());
ROS_INFO("sensor_port: %s", sensors_serial_name.c_str());
ROS_INFO("baud: %d", serial_rate);
ROS_INFO("ticks_per_meter: %.2f", ticks_per_meter);
ROS_INFO("Open ports");
if( drive_serial.open(drive_serial_name.c_str(), serial_rate) ) {
ROS_ERROR("Cant open port: %s:%d", drive_serial_name.c_str(), serial_rate);
return -1;
}
if( sensors_serial.open(sensors_serial_name.c_str(), serial_rate) ) {
ROS_WARN("Cant open port: %s:%d", sensors_serial_name.c_str(), serial_rate);
}
#if 0
orcp2::ORCP2 orcp(drive_serial);
for(int i=0; i<7; i++) {
int val = i%2;
printf("%d\n", val);
orcp.digitalWrite(13, val);
orv::time::sleep(500);
}
#endif
boost::thread drv_thread(&drive_thread);
boost::thread snsr_thread(&sensors_thread);
while (ros::ok()) {
ros::spin();
}
global_stop = true;
drv_thread.join();
snsr_thread.join();
drive_serial.close();
sensors_serial.close();
ROS_INFO("End");
return 0;
}
