/**
* Handle input command.
*
* This function should return 0, even if the command failed.
* Return of != 0 means the event loop will quit.
*/
static int handle_input_command(const dstring* string) {
dstrlist* list;
dstring** argv;
unsigned int argc;
int error;
ENetPacket* packet;
if (string->len == 0) {
/* empty command */
return 0;
} else if (string->data[0] == '/') {
/* local command */
/* parse the command */
list = dstrlex_parse(string, &error);
if (list == NULL) {
fprintf(stderr, "Failed to parse command string '%s': %s\n", string->data, dstrlex_errstr(error));
return 0;
}
/* convert list to vector */
argc = list->size;
argv = dlist_tovector(list);
dlist_free(list);
/* select command to execute */
if (dcmpcs(argv[0], "/connect") == 0) {
error = cmd_connect(argc, argv);
} else if (dcmpcs(argv[0], "/disconnect") == 0) {
error = cmd_disconnect(argc, argv);
} else if (dcmpcs(argv[0], "/quit") == 0) {
error = cmd_quit(argc, argv);
} else if (dcmpcs(argv[0], "/mute") == 0) {
error = cmd_mute(argc, argv);
} else if (dcmpcs(argv[0], "/deafen") == 0) {
error = cmd_deafen(argc, argv);
} else {
fprintf(stderr, "Unknown command '%s'\n", argv[0]->data);
error = 0;
}
dvec_free(argv);
return error;
} else if (client.state == SVCECLIENT_STATE_CONNECTED) {
/* send to server if connected */
mutex_lock(&client.network_lock);
packet = enet_packet_create(string->data, string->len, ENET_PACKET_FLAG_RELIABLE);
enet_peer_send(client.client, 0, packet);
mutex_unlock(&client.network_lock);
} else if (client.state == SVCECLIENT_STATE_CONNECTING) {
/* server command but still connecting */
fprintf(stderr, "Can't send command to server, still connecting.\n");
} else {
/* server command but not connected */
fprintf(stderr, "Can't send command to server, not connected.\n");
}
return 0;
}
int main(int argc, char *argv[])
{
GIOChannel *pchan;
gint events;
opt_sec_level = g_strdup("low");
opt_src = NULL;
opt_dst = NULL;
opt_dst_type = g_strdup("public");
printf("# " __FILE__ " built at " __TIME__ " on " __DATE__ "\n");
fflush(stdout);
event_loop = g_main_loop_new(NULL, FALSE);
pchan = g_io_channel_unix_new(fileno(stdin));
g_io_channel_set_close_on_unref(pchan, TRUE);
events = G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL;
g_io_add_watch(pchan, events, prompt_read, NULL);
g_main_loop_run(event_loop);
cmd_disconnect(0, NULL);
fflush(stdout);
g_io_channel_unref(pchan);
g_main_loop_unref(event_loop);
g_free(opt_src);
g_free(opt_dst);
g_free(opt_sec_level);
return EXIT_SUCCESS;
}
void sigint_handler(int sig)
{
ROS_WARN("Exiting...");
g_thread = false;
stop();
sleep(5);
cmd_disconnect();
ros::shutdown();
}
int main(int argc, char *argv[])
{
GIOChannel *pchan;
gint events;
const char *hci = "hci0";
opt_sec_level = g_strdup("low");
if (argc > 1)
hci = argv[1];
if (parse_dev_src(hci, &opt_src, &opt_src_idx)) {
fprintf(stderr,"%s: expected optional argument 'hciX' valid and up\n", argv[0]);
resp_error(err_BAD_HCI);
return EXIT_FAILURE;
}
DBG("Using controller hci%d addr:%s", opt_src_idx, opt_src);
opt_dst = NULL;
opt_dst_type = g_strdup("public");
DBG(__FILE__ " built at " __TIME__ " on " __DATE__);
mgmt_setup();
event_loop = g_main_loop_new(NULL, FALSE);
pchan = g_io_channel_unix_new(fileno(stdin));
g_io_channel_set_close_on_unref(pchan, TRUE);
events = G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL;
g_io_add_watch(pchan, events, prompt_read, NULL);
DBG("Starting loop");
g_main_loop_run(event_loop);
DBG("Exiting loop");
cmd_disconnect(0, NULL);
fflush(stdout);
g_io_channel_unref(pchan);
g_main_loop_unref(event_loop);
g_free(opt_src);
g_free(opt_dst);
g_free(opt_dst_type);
g_free(opt_sec_level);
mgmt_unregister_index(mgmt_master, opt_src_idx);
mgmt_cancel_index(mgmt_master, opt_src_idx);
mgmt_unref(mgmt_master);
mgmt_master = NULL;
return EXIT_SUCCESS;
}
void cmd_map(){
// The MAP command. Disconnects, loads a new map and connects if okay.
cmd_disconnect();
if(internal_setmapname(parse_words[1])==0){
if(load_map(INTERNAL_WAD)==0){
player_connected=1;
player_con=0; // <-- Bring up the console.
map_type=MAP_IWAD;
}
}
else{
con_printf("bad map name");
}
}
void clears_chat_sessions(void **state)
{
chat_sessions_init();
roster_create();
chat_session_recipient_active("*****@*****.**", "laptop", FALSE);
chat_session_recipient_active("*****@*****.**", "work", FALSE);
will_return(connection_get_status, JABBER_CONNECTED);
will_return(connection_get_fulljid, "*****@*****.**");
expect_any_cons_show();
gboolean result = cmd_disconnect(NULL, CMD_DISCONNECT, NULL);
assert_true(result);
ChatSession *session1 = chat_session_get("*****@*****.**");
ChatSession *session2 = chat_session_get("*****@*****.**");
assert_null(session1);
assert_null(session2);
}
void cmd_pwad(){
// Disconnects, loads a patch wad and connects if okay.
cmd_disconnect();
if(internal_setmapname(parse_words[2])==0){
if(internal_setwadpath(PATCH_WAD,parse_words[1])==0){
if(wad_add(PATCH_WAD)==0){
if(load_map(PATCH_WAD)==0){
player_connected=1;
player_con=0; // <-- Bring up the console.
map_type=MAP_PWAD;
}
}
}
else{
con_printf("bad wad name");
}
}
else{
con_printf("bad map name");
}
}
int interactive(const char *src, const char *dst,
const char *dst_type, int psm)
{
guint input;
guint signal;
opt_sec_level = g_strdup("low");
opt_src = g_strdup(src);
opt_dst = g_strdup(getChannelMac());
opt_dst_type = g_strdup("random");
opt_psm = psm;
prompt = g_string_new(NULL);
event_loop = g_main_loop_new(NULL, FALSE);
input = setup_standard_input();
signal = setup_signalfd();
rl_attempted_completion_function = commands_completion;
rl_erase_empty_line = 1;
rl_callback_handler_install(get_prompt(), parse_line);
g_timeout_add(1000,writehnd,NULL);
curl_global_init(CURL_GLOBAL_ALL);
g_main_loop_run(event_loop);
rl_callback_handler_remove();
cmd_disconnect(0, NULL);
g_source_remove(input);
g_source_remove(signal);
g_main_loop_unref(event_loop);
g_string_free(prompt, TRUE);
g_free(opt_src);
g_free(opt_dst);
g_free(opt_sec_level);
return 0;
}
/* SYNTAX: QUIT [<message>] */
static void cmd_quit(const char *data)
{
GSList *tmp, *next;
const char *quitmsg;
char *str;
g_return_if_fail(data != NULL);
quitmsg = *data != '\0' ? data :
settings_get_str("quit_message");
/* disconnect from every server */
for (tmp = servers; tmp != NULL; tmp = next) {
next = tmp->next;
str = g_strdup_printf("* %s", quitmsg);
cmd_disconnect(str, tmp->data);
g_free(str);
}
signal_emit("gui exit", 0);
}
int interactive(const gchar *src, const gchar *dst,
const gchar *dst_type, int psm)
{
GIOChannel *pchan;
gint events;
opt_sec_level = g_strdup("low");
opt_src = g_strdup(src);
opt_dst = g_strdup(dst);
opt_dst_type = g_strdup(dst_type);
opt_psm = psm;
prompt = g_string_new(NULL);
event_loop = g_main_loop_new(NULL, FALSE);
pchan = g_io_channel_unix_new(fileno(stdin));
g_io_channel_set_close_on_unref(pchan, TRUE);
events = G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL;
g_io_add_watch(pchan, events, prompt_read, NULL);
rl_attempted_completion_function = commands_completion;
rl_callback_handler_install(get_prompt(), parse_line);
g_main_loop_run(event_loop);
rl_callback_handler_remove();
cmd_disconnect(0, NULL);
g_io_channel_unref(pchan);
g_main_loop_unref(event_loop);
g_string_free(prompt, TRUE);
g_free(opt_src);
g_free(opt_dst);
g_free(opt_sec_level);
return 0;
}
int interactive(const gchar *src, const gchar *dst, int psm)
{
GIOChannel *pchan;
gint events;
opt_sec_level = g_strdup("low");
inp = g_malloc0(INPUT_SIZE + 1);
opt_src = g_strdup(src);
opt_dst = g_strdup(dst);
opt_psm = psm;
prompt = g_string_new(NULL);
event_loop = g_main_loop_new(NULL, FALSE);
pchan = g_io_channel_unix_new(fileno(stdin));
g_io_channel_set_close_on_unref(pchan, TRUE);
g_io_channel_set_encoding(pchan, NULL, NULL);
g_io_channel_set_buffered(pchan, FALSE);
events = G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL;
g_io_add_watch(pchan, events, prompt_read, NULL);
printf("\r%s", get_prompt());
fflush(stdout);
g_main_loop_run(event_loop);
cmd_disconnect(0, NULL);
g_io_channel_unref(pchan);
g_main_loop_unref(event_loop);
g_string_free(prompt, TRUE);
g_free(opt_src);
g_free(opt_dst);
g_free(opt_sec_level);
g_free(inp);
return 0;
}
int exec_parsed(){
// Execs a parsed command line.
int error=0,i,got=-1,n;
// Now execute the command.
if(parse_nwords!=0){
// Search for a matching known command.
for(i=0;i<max_cmds;i++){
if(strcmp(con_cmds[i].name,parse_words[0])==0){
got=i;
}
}
if(got>=0){
// Make sure that we have the correct number of arguments for a command.
if(con_cmds[got].type=='C'){
if(parse_nwords!=con_cmds[got].max+1){
got=-2;
}
}
// If we have a variable, deal with it here.
else{
// No arguments means view the variable.
if(parse_nwords==1){
view_variable(got);
got=1000;
}
// One argument means set the variable.
if(parse_nwords==2){
// Find the value to set the variable to.
if(con_cmds[got].type=='F'){
// It's a FIXED variable.
n=ftofix(atof(parse_words[1]));
}
if(con_cmds[got].type=='I'){
// It's an INT.
n=atoi(parse_words[1]);
}
if(con_cmds[got].type=='S'){
// It's a string.
internal_setstring(con_cmds[got].s_mirror,parse_words[1]);
}
else{
// Put the value within limits.
if(n<con_cmds[got].min){n=con_cmds[got].min;}
if(n>con_cmds[got].max){n=con_cmds[got].max;}
// Set the mirror to the value.
con_cmds[got].mirror=n;
}
}
// If we don't have one or two arguments, then its an error.
if(parse_nwords>2){
got=-2;
}
}
}
// Execute an actual command, or set a given variable.
switch(got){
case -1: internal_error();break;
case 0: cmd_clear();break;
case 1: player_crosshair=n;break;
case 2: player_crosshair_color=n;break;
case 3: cmd_dir();break;
case 4: cmd_disconnect();break;
case 5: cmd_exec();break;
case 6: player_floors=n;break;
case 7: player_fov_degrees=fixtof(n);break;
case 8: player_fps=n;break;
case 9: player_gamma=n;cmd_gamma();break;
case 10: player_gfx_mode=n;cmd_gfx_mode();break;
case 11: cmd_gfx_modes();break;
case 12: player_heightfix=n;break;
case 13: player_invert=n;break;
case 14: light_con=n;break;
case 15: light_depth=fixtof(n);break;
case 16: cmd_loopdemo();break;
case 17: cmd_map();break;
case 18: cmd_mapinfo();break;
case 19: player_map_rotate=n;break;
case 20: player_map=n;break;
case 21: player_map_size=n;break;
case 22: cmd_mem();break;
case 23: internal_setstring(player_pic_con,parse_words[1]);
cmd_pic_con();break;
case 24: cmd_playdemo();break;
case 25: cmd_pwad();break;
case 26: cmd_quit();break;
case 27: cmd_recdemo();break;
case 28: player_r_grad=n;break;
case 29: player_r_gun=n;break;
case 30: cmd_stop();break;
case 31: cmd_syntax();break;
case 32: cmd_time();break;
case 33: cmd_timerefresh();break;
case 34: player_trace=1;break;
case 35: cmd_viewpic();break;
case 36: cmd_viewtex();break;
case 37: player_vsync=n;break;
case 38: cmd_wads();break;
case 39: player_walls=n;break;
case 40: player_windowsize=n;break;
}
}
// Return the error value. DEBUG: Do we use this?
return(error);
}
int main( int argc, char **argv )
{
ros::init(argc, argv, "wsg_50");
ros::NodeHandle nh("~");
signal(SIGINT, sigint_handler);
std::string ip, protocol, com_mode;
int port, local_port;
double rate, grasping_force;
bool use_udp = false;
nh.param("ip", ip, std::string("192.168.1.20"));
nh.param("port", port, 1000);
nh.param("local_port", local_port, 1501);
nh.param("protocol", protocol, std::string(""));
nh.param("com_mode", com_mode, std::string(""));
nh.param("rate", rate, 1.0); // With custom script, up to 30Hz are possible
nh.param("grasping_force", grasping_force, 0.0);
if (protocol == "udp")
use_udp = true;
else
protocol = "tcp";
if (com_mode == "script")
g_mode_script = true;
else if (com_mode == "auto_update")
g_mode_periodic = true;
else {
com_mode = "polling";
g_mode_polling = true;
}
ROS_INFO("Connecting to %s:%d (%s); communication mode: %s ...", ip.c_str(), port, protocol.c_str(), com_mode.c_str());
// Connect to device using TCP/USP
int res_con;
if (!use_udp)
res_con = cmd_connect_tcp( ip.c_str(), port );
else
res_con = cmd_connect_udp(local_port, ip.c_str(), port );
if (res_con == 0 ) {
ROS_INFO("Gripper connection stablished");
// Services
ros::ServiceServer moveSS, graspSS, releaseSS, homingSS, stopSS, ackSS, incrementSS, setAccSS, setForceSS;
if (g_mode_script || g_mode_polling) {
moveSS = nh.advertiseService("move", moveSrv);
graspSS = nh.advertiseService("grasp", graspSrv);
releaseSS = nh.advertiseService("release", releaseSrv);
homingSS = nh.advertiseService("homing", homingSrv);
stopSS = nh.advertiseService("stop", stopSrv);
ackSS = nh.advertiseService("ack", ackSrv);
incrementSS = nh.advertiseService("move_incrementally", incrementSrv);
setAccSS = nh.advertiseService("set_acceleration", setAccSrv);
setForceSS = nh.advertiseService("set_force", setForceSrv);
}
// Subscriber
ros::Subscriber sub_position, sub_speed;
if (g_mode_script || g_mode_periodic)
sub_position = nh.subscribe("goal_position", 5, position_cb);
if (g_mode_script)
sub_speed = nh.subscribe("goal_speed", 5, speed_cb);
// Publisher
g_pub_state = nh.advertise<wsg_50_common::Status>("status", 1000);
g_pub_joint = nh.advertise<sensor_msgs::JointState>("joint_states", 10);
if (g_mode_script || g_mode_periodic)
g_pub_moving = nh.advertise<std_msgs::Bool>("moving", 10);
ROS_INFO("Ready to use. Homing and taring now...");
homing();
ros::Duration(0.5).sleep();
doTare();
if (grasping_force > 0.0) {
ROS_INFO("Setting grasping force limit to %5.1f", grasping_force);
setGraspingForceLimit(grasping_force);
}
ROS_INFO("Init done. Starting timer/thread with target rate %.1f.", rate);
std::thread th;
ros::Timer tmr;
if (g_mode_polling || g_mode_script)
tmr = nh.createTimer(ros::Duration(1.0/rate), timer_cb);
if (g_mode_periodic)
th = std::thread(read_thread, (int)(1000.0/rate));
ros::spin();
} else {
ROS_ERROR("Unable to connect, please check the port and address used.");
}
ROS_INFO("Exiting...");
g_mode_periodic = false;
g_mode_script = false;
g_mode_polling = false;
sleep(1);
cmd_disconnect();
return 0;
}
int main(int argc, char **argv)
{
char addr[18];
char *handle, *value;
int ret, choice;
printf("*********************************\n");
printf("**** Nod Labs test framework ****\n");
printf("*********************************\n");
/* initialize a glib event loop */
event_loop = g_main_loop_new(NULL, FALSE);
printf("Scanning for all BTLE devices in proximity...");
printf("Press ^C to stop scanning\n");
if(ret = cmd_lescan(0) < 0) {
printf("Scanning failed!! Quitting %d\n", ret);
exit(-1);
}
printf("Enter the device to connect: ");
scanf("%s", addr);
ret = 0;
if (ret = cmd_connect(addr, NULL) < 0) {
printf("Connection failed %d\n", ret);
exit(-2);
} else {
printf("Connected to [%s]\n", addr);
}
printf("SERVICES:\n");
discover_services((gpointer)attrib);
printf("\nCHARACTERISTICS:\n");
discover_characteristics((gpointer)attrib, CHAR_START, CHAR_END);
/* IMP: If the native bluez stack is running while this test runs, then the
* keys get cached in the system(on which this is running). Next time, when this
* test case is being run, it will fail to change the security level to
* anything other than "low". To resolve this issue, we need to check for
* the paired devices in bluetoothctl (using cmd 'show devices') and remove
* the device from bluez device cache.
*
* HIGH security level is needed for initiating the bonding with the device. This
* is important as HID services need the link to be encrypted.
*/
if (cmd_set_sec_level("high") < 0) {
printf("Failed to set the security level to HIGH\n");
exit(-1);
} else {
printf("Security level set to HIGH\n");
}
/* Now, enable battery and nControl notifications in the ring */
control_service(non_os_indexes, GET_SZ(non_os_indexes), ENABLE_NOTIFICATION);
/*
* TODO: Make sure the below mode list is up-to-date with
* the numbers used in the f/w
*/
ret = 0;
printf("Which mode you want to enable?\n");
printf("1. TTM\n");
printf("2. Free pointer\n");
printf("3. OpenSpatial (Pose6D & Buttons)\n");
printf("0. Exit\n");
printf("Enter your choice: ");
scanf("%d", &choice);
printf("\n");
if (!choice) {
exit(0);
}
/* update the mode accordingly */
change_mode((gpointer)attrib, choice);
/* We have an active connection, with the data ready to be received */
set_state(STATE_CONNACTIVE);
/* let the signal handler do necessary cleanup before proceeding to disconnect */
wait_for_cleanup = 1;
/* below loop will control the event loop for receiving the data */
g_main_loop_run(event_loop);
while (wait_for_cleanup);
printf("Disconnecting from device [%s]\n", addr);
cmd_disconnect();
/* un-initialize glib event loop */
g_main_loop_unref(event_loop);
return 0;
}
int main( int argc, char **argv )
{
ros::init(argc, argv, "wsg_50");
ros::NodeHandle nh;
std::string ip;
int port;
ROS_INFO("WSG 50 - ROS NODE");
nh.param("wsg_50_tcp/ip", ip, std::string("192.168.1.20"));
nh.param("wsg_50_tcp/port", port, 1000);
// Connect to device using TCP
if( cmd_connect_tcp( ip.c_str(), port ) == 0 )
{
ROS_INFO("TCP connection stablished");
// Services
ros::ServiceServer moveSS = nh.advertiseService("wsg_50/move", moveSrv);
ros::ServiceServer graspSS = nh.advertiseService("wsg_50/grasp", graspSrv);
ros::ServiceServer releaseSS = nh.advertiseService("wsg_50/release", releaseSrv);
ros::ServiceServer homingSS = nh.advertiseService("wsg_50/homing", homingSrv);
ros::ServiceServer stopSS = nh.advertiseService("wsg_50/stop", stopSrv);
ros::ServiceServer ackSS = nh.advertiseService("wsg_50/ack", ackSrv);
ros::ServiceServer incrementSS = nh.advertiseService("wsg_50/move_incrementally", incrementSrv);
ros::ServiceServer setAccSS = nh.advertiseService("wsg_50/set_acceleration", setAccSrv);
ros::ServiceServer setForceSS = nh.advertiseService("wsg_50/set_force", setForceSrv);
// Publisher
ros::Publisher state_pub = nh.advertise<wsg_50_common::Status>("wsg_50/status", 1000);
ros::Publisher joint_states_pub = nh.advertise<sensor_msgs::JointState>("/joint_states", 10);
ROS_INFO("Ready to use.");
homing();
ros::Rate loop_rate(1); // loop at 1Hz
while( ros::ok() ){
//Loop waiting for orders and updating the state
//Create the msg to send
wsg_50_common::Status status_msg;
//Get state values
const char * aux;
aux = systemState();
int op = getOpening();
int acc = getAcceleration();
int force = getGraspingForceLimit();
std::stringstream ss;
ss << aux;
status_msg.status = ss.str();
status_msg.width = op;
status_msg.acc = acc;
status_msg.force = force;
state_pub.publish(status_msg);
sensor_msgs::JointState joint_states;
joint_states.header.stamp = ros::Time::now();;
joint_states.header.frame_id = "wsg_50_gripper_base_link";
joint_states.name.push_back("wsg_50_gripper_base_joint_gripper_left");
joint_states.name.push_back("wsg_50_gripper_base_joint_gripper_right");
joint_states.position.resize(2);
joint_states.position[0] = -op/2000.0;
joint_states.position[1] = op/2000.0;
//joint_states.velocity.resize(2);
//joint_states.velocity[0];
//joint_states.velocity[1];
joint_states.effort.resize(2);
joint_states.effort[0] = force;
joint_states.effort[1] = force;
joint_states_pub.publish(joint_states);
loop_rate.sleep();
ros::spinOnce();
}
}else{
ROS_ERROR("Unable to connect via TCP, please check the port and address used.");
}
// Disconnect - won't be executed atm. as the endless loop in test()
// will never return.
cmd_disconnect();
return 0;
}