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