void initialize(UAS &uas_, ros::NodeHandle &nh, diagnostic_updater::Updater &diag_updater) { bool pose_with_covariance; bool listen_tf; uas = &uas_; sp_nh = ros::NodeHandle(nh, "position"); sp_nh.param("vision/pose_with_covariance", pose_with_covariance, false); sp_nh.param("vision/listen_tf", listen_tf, false); sp_nh.param<std::string>("vision/frame_id", frame_id, "local_origin"); sp_nh.param<std::string>("vision/child_frame_id", child_frame_id, "vision"); sp_nh.param("vision/tf_rate_limit", tf_rate, 50.0); ROS_DEBUG_STREAM_NAMED("position", "Vision pose topic type: " << ((pose_with_covariance)? "PoseWithCovarianceStamped" : "PoseStamped")); if (listen_tf) { ROS_INFO_STREAM_NAMED("position", "Listen to vision transform " << frame_id << " -> " << child_frame_id); tf_start("VisionTF", &VisionPoseEstimatePlugin::send_vision_transform); } else if (pose_with_covariance) vision_sub = sp_nh.subscribe("vision", 10, &VisionPoseEstimatePlugin::vision_cov_cb, this); else vision_sub = sp_nh.subscribe("vision", 10, &VisionPoseEstimatePlugin::vision_cb, this); }
void initialize(UAS &uas_) { bool pose_with_covariance; bool listen_tf; bool listen_twist; bool listen_att_ctrl; uas = &uas_; sp_nh.param("listen_twist", listen_twist, false); sp_nh.param("pose_with_covariance", pose_with_covariance, false); // may be used to mimic attitude of an object, a gesture, etc. sp_nh.param("listen_tf", listen_tf, false); sp_nh.param("listen_att_ctrl", listen_att_ctrl, true); sp_nh.param<std::string>("frame_id", frame_id, "local_origin"); sp_nh.param<std::string>("child_frame_id", child_frame_id, "attitude"); sp_nh.param("tf_rate_limit", tf_rate, 10.0); sp_nh.param("reverse_throttle", reverse_throttle, false); if (listen_tf) { ROS_INFO_STREAM_NAMED("attitude", "Listen to desired attitude transform " << frame_id << " -> " << child_frame_id); tf_start("AttitudeSpTF", &SetpointAttitudePlugin::send_attitude_transform); } else if (listen_twist) { ROS_DEBUG_NAMED("attitude", "Setpoint attitude topic type: Twist"); att_sub = sp_nh.subscribe("cmd_vel", 10, &SetpointAttitudePlugin::twist_cb, this); } else if (pose_with_covariance) { ROS_DEBUG_NAMED("attitude", "Setpoint attitude topic type: PoseWithCovarianceStamped"); att_sub = sp_nh.subscribe("attitude", 10, &SetpointAttitudePlugin::pose_cov_cb, this); } else if (listen_att_ctrl) { att_sub = sp_nh.subscribe("attitude_ctrl", 10, &SetpointAttitudePlugin::att_ctrl_cb, this); } else { ROS_DEBUG_NAMED("attitude", "Setpoint attitude topic type: PoseStamped"); att_sub = sp_nh.subscribe("attitude", 10, &SetpointAttitudePlugin::pose_cb, this); } // throttle_sub = sp_nh.subscribe("att_throttle", 10, &SetpointAttitudePlugin::throttle_cb, this); }
void initialize(UAS &uas_) { bool listen_tf; uas = &uas_; sp_nh.param("listen_tf", listen_tf, false); sp_nh.param<std::string>("frame_id", frame_id, "local_origin"); sp_nh.param<std::string>("child_frame_id", child_frame_id, "setpoint"); sp_nh.param("tf_rate_limit", tf_rate, 50.0); if (listen_tf) { ROS_INFO_STREAM_NAMED("setpoint", "Listen to position setpoint transform " << frame_id << " -> " << child_frame_id); tf_start("PositionSpTF", &SetpointPositionPlugin::send_setpoint_transform); } else { setpoint_sub = sp_nh.subscribe("local", 10, &SetpointPositionPlugin::setpoint_cb, this); } }
void initialize(UAS &uas_, ros::NodeHandle &nh, diagnostic_updater::Updater &diag_updater) { bool listen_tf; uas = &uas_; sp_nh = ros::NodeHandle(nh, "setpoint"); sp_nh.param("position/listen_tf", listen_tf, false); sp_nh.param<std::string>("position/frame_id", frame_id, "local_origin"); sp_nh.param<std::string>("position/child_frame_id", child_frame_id, "setpoint"); sp_nh.param("position/tf_rate_limit", tf_rate, 50.0); if (listen_tf) { ROS_INFO_STREAM_NAMED("setpoint", "Listen to position setpoint transform " << frame_id << " -> " << child_frame_id); tf_start("PositionSpTF", &SetpointPositionPlugin::send_setpoint_transform); } else { setpoint_sub = sp_nh.subscribe("local_position", 10, &SetpointPositionPlugin::setpoint_cb, this); } }
void initialize(UAS &uas_, ros::NodeHandle &nh, diagnostic_updater::Updater &diag_updater) { bool pose_with_covariance; bool listen_tf; bool listen_twist; uas = &uas_; sp_nh = ros::NodeHandle(nh, "setpoint"); sp_nh.param("attitude/listen_twist", listen_twist, true); sp_nh.param("attitude/pose_with_covariance", pose_with_covariance, false); // may be used to mimic attitude of an object, a gesture, etc. sp_nh.param("attitude/listen_tf", listen_tf, false); sp_nh.param<std::string>("attitude/frame_id", frame_id, "local_origin"); sp_nh.param<std::string>("attitude/child_frame_id", child_frame_id, "attitude"); sp_nh.param("attitude/tf_rate_limit", tf_rate, 10.0); if (listen_tf) { ROS_INFO_STREAM_NAMED("attitude", "Listen to desired attitude transform " << frame_id << " -> " << child_frame_id); tf_start("AttitudeSpTF", &SetpointAttitudePlugin::send_attitude_transform); } else if (listen_twist) { ROS_DEBUG_NAMED("attitude", "Setpoint attitude topic type: Twist"); att_sub = sp_nh.subscribe("att_vel", 10, &SetpointAttitudePlugin::twist_cb, this); } else if (pose_with_covariance) { ROS_DEBUG_NAMED("attitude", "Setpoint attitude topic type: PoseWithCovarianceStamped"); att_sub = sp_nh.subscribe("attitude", 10, &SetpointAttitudePlugin::pose_cov_cb, this); } else { ROS_DEBUG_NAMED("attitude", "Setpoint attitude topic type: PoseStamped"); att_sub = sp_nh.subscribe("attitude", 10, &SetpointAttitudePlugin::pose_cb, this); } }
static long tf_ctrl_device_ioctl(struct file *file, unsigned int ioctl_num, unsigned long ioctl_param) { int result = S_SUCCESS; struct tf_pa_ctrl pa_ctrl; struct tf_device *dev = tf_get_device(); dpr_info("%s(%p, %u, %p)\n", __func__, file, ioctl_num, (void *) ioctl_param); mutex_lock(&dev->dev_mutex); if (ioctl_num != IOCTL_TF_PA_CTRL) { dpr_err("%s(%p): ioctl number is invalid (%p)\n", __func__, file, (void *)ioctl_num); result = -EFAULT; goto exit; } if ((ioctl_param & 0x3) != 0) { dpr_err("%s(%p): ioctl command message pointer is not word " "aligned (%p)\n", __func__, file, (void *)ioctl_param); result = -EFAULT; goto exit; } if (copy_from_user(&pa_ctrl, (struct tf_pa_ctrl *)ioctl_param, sizeof(struct tf_pa_ctrl))) { dpr_err("%s(%p): cannot access ioctl parameter (%p)\n", __func__, file, (void *)ioctl_param); result = -EFAULT; goto exit; } switch (pa_ctrl.nPACommand) { case TF_PA_CTRL_START: { struct tf_shmem_desc *shmem_desc = NULL; u32 shared_mem_descriptors[TF_MAX_COARSE_PAGES]; u32 descriptor_count; u32 offset; struct tf_connection *connection; dpr_info("%s(%p): Start the SMC PA (%d bytes) with conf " "(%d bytes)\n", __func__, file, pa_ctrl.pa_size, pa_ctrl.conf_size); connection = tf_conn_from_file(file); if (dev->workspace_addr == 0) { result = -ENOMEM; goto start_exit; } result = tf_validate_shmem_and_flags( (u32)pa_ctrl.conf_buffer, pa_ctrl.conf_size, TF_SHMEM_TYPE_READ); if (result != 0) goto start_exit; offset = 0; result = tf_map_shmem( connection, (u32)pa_ctrl.conf_buffer, TF_SHMEM_TYPE_READ, true, /* in user space */ shared_mem_descriptors, &offset, pa_ctrl.conf_size, &shmem_desc, &descriptor_count); if (result != 0) goto start_exit; if (descriptor_count > 1) { dpr_err("%s(%p): configuration file is too long (%d)\n", __func__, file, descriptor_count); result = -ENOMEM; goto start_exit; } result = tf_start(&dev->sm, dev->workspace_addr, dev->workspace_size, pa_ctrl.pa_buffer, pa_ctrl.pa_size, shared_mem_descriptors[0], offset, pa_ctrl.conf_size); if (result) dpr_err("SMC: start failed\n"); else dpr_info("SMC: started\n"); start_exit: tf_unmap_shmem(connection, shmem_desc, true); /* full cleanup */ break; } case TF_PA_CTRL_STOP: dpr_info("%s(%p): Stop the SMC PA\n", __func__, file); result = tf_power_management(&dev->sm, TF_POWER_OPERATION_SHUTDOWN); if (result) dpr_err("SMC: stop failed [0x%x]\n", result); else dpr_info("SMC: stopped\n"); break; default: result = -EOPNOTSUPP; break; } exit: mutex_unlock(&dev->dev_mutex); return result; }