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);
}
