int main(int argc, char * argv[]) { // Pass command line arguments to rclcpp. rclcpp::init(argc, argv); // Initialize default demo parameters size_t depth = 10; rmw_qos_reliability_policy_t reliability_policy = RMW_QOS_POLICY_RELIABILITY_RELIABLE; rmw_qos_history_policy_t history_policy = RMW_QOS_POLICY_HISTORY_KEEP_ALL; bool show_camera = true; // Configure demo parameters with command line options. if (!parse_command_options(argc, argv, &depth, &reliability_policy, &history_policy, &show_camera)) { return 0; } if (show_camera) { // Initialize an OpenCV named window called "showimage". cvNamedWindow("showimage", CV_WINDOW_AUTOSIZE); } // Initialize a ROS node. auto node = rclcpp::node::Node::make_shared("showimage"); // Set quality of service profile based on command line options. rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default; // The history policy determines how messages are saved until the message is taken by the reader. // KEEP_ALL saves all messages until they are taken. // KEEP_LAST enforces a limit on the number of messages that are saved, specified by the "depth" // parameter. custom_qos_profile.history = history_policy; // Depth represents how many messages to store in history when the history policy is KEEP_LAST. custom_qos_profile.depth = depth; // The reliability policy can be reliable, meaning that the underlying transport layer will try // ensure that every message gets received in order, or best effort, meaning that the transport // makes no guarantees about the order or reliability of delivery. custom_qos_profile.reliability = reliability_policy; auto callback = [show_camera](const sensor_msgs::msg::Image::SharedPtr msg) { show_image(msg, show_camera); }; // Initialize a subscriber that will receive the ROS Image message to be displayed. auto sub = node->create_subscription<sensor_msgs::msg::Image>( "image", callback, custom_qos_profile); rclcpp::spin(node); return 0; }
struct task_details *figure_out_what_to_do( int *returncode, int narg, char **opts) { int rc = RC_NORMAL, off, *int_p = 0, server_set = 0, errlen; char *unrecognized = 0; struct task_details *plan = 0; struct option_set opset[] = { { OP_SERVER, OP_TYPE_CHAR, OP_FL_BLANK, FL_SERVER, 0, DEF_SERVER, 0, 0 }, { OP_SERVER, OP_TYPE_CHAR, OP_FL_BLANK, FL_SERVER_2, 0, DEF_SERVER, 0, 0 }, { OP_USER, OP_TYPE_CHAR, OP_FL_BLANK, FL_USER, 0, DEF_USER, 0, 0 }, { OP_USER, OP_TYPE_CHAR, OP_FL_BLANK, FL_USER_2, 0, DEF_USER, 0, 0 }, { OP_PORT, OP_TYPE_INT, OP_FL_BLANK, FL_PORT, 0, DEF_PORT, 0, 0 }, { OP_PORT, OP_TYPE_INT, OP_FL_BLANK, FL_PORT_2, 0, DEF_PORT, 0, 0 }, { OP_HOST, OP_TYPE_CHAR, OP_FL_BLANK, FL_HOST, 0, DEF_HOST, 0, 0 }, { OP_HOST, OP_TYPE_CHAR, OP_FL_BLANK, FL_HOST_2, 0, DEF_HOST, 0, 0 }, { OP_IPV4, OP_TYPE_FLAG, OP_FL_BLANK, FL_IPV4, 0, DEF_IPV4, 0, 0 }, { OP_IPV4, OP_TYPE_FLAG, OP_FL_BLANK, FL_IPV4_2, 0, DEF_IPV4, 0, 0 }, { OP_IPV6, OP_TYPE_FLAG, OP_FL_BLANK, FL_IPV6, 0, DEF_IPV6, 0, 0 }, { OP_IPV6, OP_TYPE_FLAG, OP_FL_BLANK, FL_IPV6_2, 0, DEF_IPV6, 0, 0 }, { OP_LOGFILE, OP_TYPE_CHAR, OP_FL_BLANK, FL_LOGFILE, 0, DEF_LOGFILE, 0, 0 }, { OP_LOGFILE, OP_TYPE_CHAR, OP_FL_BLANK, FL_LOGFILE_2, 0, DEF_LOGFILE, 0, 0 }, { OP_MODE, OP_TYPE_INT, OP_FL_BLANK, FL_MODE, 0, DEF_MODE, 0, 0 }, { OP_MODE, OP_TYPE_INT, OP_FL_BLANK, FL_MODE_2, 0, DEF_MODE, 0, 0 }, { OP_DEBUG, OP_TYPE_INT, OP_FL_BLANK, FL_DEBUG, 0, DEF_DEBUG, 0, 0 }, { OP_GROUP, OP_TYPE_CHAR, OP_FL_BLANK, FL_GROUP, 0, DEF_GROUP, 0, 0 }, { OP_GROUP, OP_TYPE_CHAR, OP_FL_BLANK, FL_GROUP_2, 0, DEF_GROUP, 0, 0 }, { OP_TRUNC, OP_TYPE_FLAG, OP_FL_BLANK, FL_TRUNC, 0, DEF_TRUNC, 0, 0 }, { OP_TRUNC, OP_TYPE_FLAG, OP_FL_BLANK, FL_TRUNC_2, 0, DEF_TRUNC, 0, 0 }, { OP_APPEND, OP_TYPE_FLAG, OP_FL_BLANK, FL_APPEND, 0, DEF_APPEND, 0, 0 }, { OP_APPEND, OP_TYPE_FLAG, OP_FL_BLANK, FL_APPEND_2, 0, DEF_APPEND, 0, 0 }, { OP_HELP, OP_TYPE_FLAG, OP_FL_BLANK, FL_HELP, 0, DEF_HELP, 0, 0 }, }; struct option_set *co = 0, *co_ap = 0, *co_tr = 0, *ipv4 = 0, *ipv6 = 0; struct word_chain *extra_opts = 0, *walk = 0; int nflags = (sizeof opset) / (sizeof opset[0]); char *sep = 0; plan = allocate_plan_data(); if( !plan) rc = ERR_MALLOC_FAILED; /* --- */ if( rc == RC_NORMAL) { extra_opts = parse_command_options( &rc, opset, nflags, narg, opts); /* Need to pull this one up to enable debug logging (other options are parsed later) */ if( rc == RC_NORMAL) { co = get_matching_option( OP_DEBUG, opset, nflags); if( !co) rc = ERR_OPT_CONFIG; else { plan->debug = *((int *) co->parsed); if( plan->debug >= DEBUG_HIGH) fprintf( stderr, "Opt #%d, debug '%d'\n", co->opt_num, *((int *) co->parsed)); } } /* --- */ for( walk = extra_opts, errlen = 0; walk; walk = walk->next) if( walk->opt) if( *walk->opt) errlen += strlen( walk->opt) + 1; if( errlen) { rc = ERR_SYNTAX; unrecognized = (char *) malloc( errlen + 1); if( !unrecognized) rc = ERR_MALLOC_FAILED; else { *unrecognized = '\0'; for( walk = extra_opts; walk; ) { if( walk->opt) strcat( unrecognized, walk->opt); walk = walk->next; if( walk) if( walk->opt) strcat( unrecognized, " "); } errlen += strlen( ERRMSG_UNRECOG_OPTIONS); plan->err_msg = (char *) malloc( errlen); if( !plan->err_msg) rc = ERR_MALLOC_FAILED; else snprintf( plan->err_msg, errlen, ERRMSG_UNRECOG_OPTIONS, unrecognized); free( unrecognized); } } /* --- */ if( plan->debug >= DEBUG_HIGH) { fprintf( stderr, "rc=%d extra(", rc); sep = ""; for( walk = extra_opts; walk; ) { fprintf( stderr, "%s%s", sep, walk->opt); walk = walk->next; if( walk) sep = " "; else sep = ""; } fprintf( stderr, ")\n"); /* Print out settings just for options included on the command line */ fprintf( stderr, "Seq Num Typ Fl Opt\n"); for( off= 0; off < nflags; off++) { co = opset + off; if( co->opt_num) { fprintf( stderr, "%2d. %3d %3d %2x ", off + 1, co->num, co->type, co->flags); fprintf( stderr, "%3d ", co->opt_num); if( co->type == OP_TYPE_INT || co->type == OP_TYPE_FLAG) { int_p = (int *) co->parsed; fprintf( stderr, "%d ", *int_p); } else if( co->type == OP_TYPE_CHAR) fprintf( stderr, "(%s) ", (char *) co->parsed); else if( co->type == OP_TYPE_FLOAT) fprintf( stderr, "%f ", *((float *) co->parsed)); else fprintf( stderr, "/?/ "); fprintf( stderr, "(%s) (%s) ", co->name, co->val); fprintf( stderr, "\n"); } } /* Print out all options settings, includes defaults for unspecified options */ fprintf( stderr, "Seq Num Typ Fl Opt\n"); for( off= 0; off < nflags; off++) { co = opset + off; fprintf( stderr, "%2d. %3d %3d %2x ", off + 1, co->num, co->type, co->flags); fprintf( stderr, "%3d ", co->opt_num); if( co->type == OP_TYPE_INT || co->type == OP_TYPE_FLAG) { int_p = (int *) co->parsed; fprintf( stderr, "%d ", *int_p); } else if( co->type == OP_TYPE_CHAR) fprintf( stderr, "(%s) ", (char *) co->parsed); else if( co->type == OP_TYPE_FLOAT) fprintf( stderr, "%f ", *((float *) co->parsed)); else fprintf( stderr, "/?/ "); fprintf( stderr, "(%s) (%s) ", co->name, co->val); fprintf( stderr, "\n"); } } } /* --- */ if( rc == RC_NORMAL) { co = get_matching_option( OP_SERVER, opset, nflags); if( !co) rc = ERR_OPT_CONFIG; else { if( plan->debug >= DEBUG_HIGH) fprintf( stderr, "Opt #%d, server '%s'\n", co->opt_num, (char *) co->parsed); server_set = co->opt_num; rc = parse_destination_value( plan, (char *) co->parsed); } } if( rc == RC_NORMAL) { co = get_matching_option( OP_USER, opset, nflags); if( !co) rc = ERR_OPT_CONFIG; else { plan->runuser = (char *) co->parsed; if( plan->debug >= DEBUG_HIGH) fprintf( stderr, "Opt #%d, run-user '%s'\n", co->opt_num, (char *) co->parsed); } } if( rc == RC_NORMAL) { co = get_matching_option( OP_GROUP, opset, nflags); if( !co) rc = ERR_OPT_CONFIG; else { plan->rungroup = (char *) co->parsed; if( plan->debug >= DEBUG_HIGH) fprintf( stderr, "Opt #%d, run-group '%s'\n", co->opt_num, (char *) co->parsed); } } if( rc == RC_NORMAL) { co = get_matching_option( OP_PORT, opset, nflags); if( !co) rc = ERR_OPT_CONFIG; else { if( co->opt_num >= server_set) plan->target_port = *((int *) co->parsed); if( plan->debug >= DEBUG_HIGH) fprintf( stderr, "Opt #%d, port '%d'\n", co->opt_num, *((int *) co->parsed)); } } if( rc == RC_NORMAL) { co = get_matching_option( OP_HOST, opset, nflags); if( !co) rc = ERR_OPT_CONFIG; else { if( co->opt_num >= server_set) plan->target_host = (char *) co->parsed; if( plan->debug >= DEBUG_HIGH) fprintf( stderr, "Opt #%d, host '%s'\n", co->opt_num, (char *) co->parsed); } } if( rc == RC_NORMAL) { co = get_matching_option( OP_IPV4, opset, nflags); if( !co) rc = ERR_OPT_CONFIG; else { if( co->flags & OP_FL_SET) plan->use_ip |= DO_IPV4; else plan->use_ip &= ~DO_IPV4; ipv4 = co; if( plan->debug >= DEBUG_HIGH) fprintf( stderr, "Opt #%d, ipv6=%x, use-ip=%x\n", co->opt_num, co->flags, plan->use_ip); } } if( rc == RC_NORMAL) { co = get_matching_option( OP_IPV6, opset, nflags); if( !co) rc = ERR_OPT_CONFIG; else { if( co->flags & OP_FL_SET) plan->use_ip |= DO_IPV6; else plan->use_ip &= ~DO_IPV6; ipv6 = co; if( plan->debug >= DEBUG_HIGH) fprintf( stderr, "Opt #%d, ipv6=%x, use-ip=%x\n", co->opt_num, co->flags, plan->use_ip); } } if( rc == RC_NORMAL) { co = get_matching_option( OP_LOGFILE, opset, nflags); if( !co) rc = ERR_OPT_CONFIG; else { plan->logfile = (char *) co->parsed; if( plan->debug >= DEBUG_HIGH) fprintf( stderr, "Opt #%d, logfile '%s'\n", co->opt_num, (char *) co->parsed); } } if( rc == RC_NORMAL) { co = get_matching_option( OP_MODE, opset, nflags); if( !co) rc = ERR_OPT_CONFIG; else { plan->logmode = *((int *) co->parsed); if( plan->debug >= DEBUG_HIGH) fprintf( stderr, "Opt #%d, logmode '%d'\n", co->opt_num, *((int *) co->parsed)); } } if( rc == RC_NORMAL) { co = get_matching_option( OP_HELP, opset, nflags); if( !co) rc = ERR_OPT_CONFIG; else { if( co->flags & OP_FL_SET) plan->show_help = 1; if( plan->debug >= DEBUG_HIGH) fprintf( stderr, "Opt #%d, help '%d'\n", co->opt_num, plan->show_help); } } if( rc == RC_NORMAL) { co_tr = get_matching_option( OP_TRUNC, opset, nflags); co_ap = get_matching_option( OP_APPEND, opset, nflags); if( !co_tr || !co_ap) rc = ERR_OPT_CONFIG; else { if( co_tr->opt_num && co_ap->opt_num) { if( co_ap->flags == OP_FL_SET) { if( co_tr->flags == OP_FL_SET) plan->openflags |= O_APPEND | O_TRUNC; else plan->openflags |= O_APPEND; } else if( co_tr->flags == OP_FL_SET) plan->openflags |= O_TRUNC; } else if( co_tr->opt_num) { if( co_tr->flags == OP_FL_SET) plan->openflags |= O_APPEND | O_TRUNC; else plan->openflags |= O_APPEND; } else if( co_ap->opt_num) { if( co_ap->flags == OP_FL_SET) plan->openflags |= O_APPEND; else plan->openflags |= O_TRUNC; } else plan->openflags |= O_APPEND; } } /* By default both IPV4 and IPV6 are disabled. If neither option was selected via * command line flags, them turn the both on so the code will take whichever one it * finds. */ if( rc == RC_NORMAL) if( !(plan->use_ip & (DO_IPV4 | DO_IPV6))) { if( ipv4->opt_num && ipv6->opt_num) rc = ERR_INVALID_DATA; else if( ipv4->opt_num) plan->use_ip |= DO_IPV6; else if( ipv6->opt_num) plan->use_ip |= DO_IPV4; else plan->use_ip |= (DO_IPV4 | DO_IPV6); if( plan->debug >= DEBUG_HIGH) fprintf( stderr, "IP check, ipv4=%d, ipv6=%d, use_ip=%x\n", ipv4->opt_num, ipv6->opt_num, plan->use_ip); } if( rc == ERR_OPT_CONFIG) plan->err_msg = "Unrecoverable internal configuration error, can't continue."; else if( rc == ERR_INVALID_DATA) plan->err_msg = "Both --no-ipv4 and --no-ipv6 specified, at least one needs to be allowed."; *returncode = rc; if( plan->debug >= DEBUG_HIGH) fprintf( stderr, "End figure-it-out() call, rc=%d\n", *returncode); return( plan); }