/**
* @brief Mouse right click on map
* @param p_x
* @param p_y
* @note If a marker is clicked, the markerClicked() signal
* containing the marker id is emitted, otherwise the context menu
* to save the new place is shown
*/
void MarbleMap::mouseRightClick(int p_x, int p_y)
{
#ifdef DBG_MARBLE_MAP
qDebug() << "MarbleMap::mouseRightClick() pt = " << p_x << ", " << p_y;
#endif
int l_id = -1;
QVector<const GeoDataPlacemark *> l_v = whichFeatureAt(QPoint(p_x, p_y));
if(0 < l_v.size()) // since we can have more placemarks (e.g. zoom too low)
{
l_id = l_v.at(0)->id(); // then, get the first one
emit markerClicked(l_id);
}
else
{
QMenu l_menu;
l_menu.addAction("Save this location?");
QPoint l_pos = mapToGlobal(QPoint(p_x, p_y)); // the menu position is relative to screen and not to widget
QAction* l_action = l_menu.exec(l_pos);
if (l_action)
{
qreal l_lng;
qreal l_lat;
geoCoordinates(p_x, p_y, l_lng, l_lat);
#ifdef DBG_MARBLE_MAP
qDebug() << "MarbleMap::mouseRightClick() going to create marker at " << l_lat << ", " << l_lng;
#endif
createMarker(l_lat, l_lng);
}
}
}
void FiducialsNode::tag_cb(int id, double x, double y, double z, double twist,
double dx, double dy, double dz, int visible) {
if (!publish_markers)
return;
visualization_msgs::Marker marker = createMarker(fiducial_namespace, id);
marker.type = visualization_msgs::Marker::CUBE;
marker.action = visualization_msgs::Marker::ADD;
marker.pose = scale_position(x, y, z, 0);
marker.scale.x = dx / scale;
marker.scale.y = dy / scale;
marker.scale.z = dz / scale;
if( visible ) {
marker.color = tag_color;
} else {
marker.color = hidden_tag_color;
}
marker.lifetime = ros::Duration();
marker_pub->publish(marker);
// publish text(ID) version of marker
char str_id[12];
snprintf(str_id, 12, "%d", id);
marker.text = str_id;
marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
marker.pose.position.z += (marker.scale.z/2.0) + 0.05; // draw text above marker
marker.color.r = marker.color.g = marker.color.b = 1.0; // white
marker.scale.x = marker.scale.y = marker.scale.z = 0.2;
marker.id = id + 10000;
marker.ns = fiducial_namespace + "_text";
marker_pub->publish(marker);
}
void
ShapeMarker::createInteractiveMarker ()
{
visualization_msgs::InteractiveMarker imarker ;
// ROS_INFO("\tcreating interactive marker for shape < %d >", shape_.id);
stringstream ss;
if(!arrows_ && !deleted_) {
ss.str("");
ss.clear() ;
ss << shape_.id ;
marker_.name = ss.str ();
marker_.header = shape_.header;
marker_.header.stamp = ros::Time::now() ;
}
else if(arrows_)
{
ROS_INFO("Second Marker... ") ;
ss.str("");
ss.clear() ;
ss << "second_marker_" <<shape_.id ;
imarker_.name = ss.str ();
imarker_.header = shape_.header;
imarker_.header.stamp = ros::Time::now() ;
}
else if(deleted_)
{
ROS_INFO("Deleted Marker... ") ;
ss.str("");
ss.clear() ;
ss << "deleted_marker_" <<shape_.id ;
deleted_imarker_.name = ss.str ();
deleted_imarker_.header = shape_.header;
deleted_imarker_.header.stamp = ros::Time::now() ;
}
visualization_msgs::InteractiveMarkerControl im_ctrl_for_second_marker;
/* create marker */
if (!deleted_ && !arrows_) {
ss.str("");
ss.clear() ;
ss.str ("");
im_ctrl.always_visible = true;
ss << "shape_" << shape_.id << "_control";
im_ctrl.name = ss.str ();
im_ctrl.description = "shape_markers";
im_ctrl.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
createMarker (im_ctrl);
marker_.controls.push_back(im_ctrl) ;
im_server_->insert (marker_ );
im_server_ ->applyChanges() ;
menu_handler_.apply (*im_server_, marker_.name);
}
else if (arrows_)
{
createMarker (im_ctrl_for_second_marker);
imarker_.controls.push_back(im_ctrl_for_second_marker) ;
im_server_->insert (imarker_ );
im_server_ ->applyChanges() ;
// menu_handler_.apply (*im_server_, imarker_.name);
}
else if(deleted_){
im_ctrl_for_second_marker.always_visible = true;
im_ctrl_for_second_marker.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
createMarker (im_ctrl_for_second_marker);
deleted_imarker_.controls.push_back(im_ctrl_for_second_marker) ;
im_server_->insert (deleted_imarker_ );
im_server_ ->applyChanges() ;
menu_handler_.apply (*im_server_, deleted_imarker_.name);
}
}
void Selection3DDisplayCustom::createMarker(int xo, int yo, int x, int y)
{
createMarker(false,x,y);
}
void FiducialsNode::location_cb(int id, double x, double y, double z,
double bearing) {
ROS_INFO("location_announce:id=%d x=%f y=%f bearing=%f",
id, x, y, bearing * 180. / 3.1415926);
visualization_msgs::Marker marker = createMarker(position_namespace, id);
ros::Time now = marker.header.stamp;
marker.type = visualization_msgs::Marker::ARROW;
marker.action = visualization_msgs::Marker::ADD;
marker.pose = scale_position(x, y, z, bearing);
marker.scale.x = 0.2 / scale;
marker.scale.y = 0.05 / scale;
marker.scale.z = 0.05 / scale;
marker.color = position_color;
marker.lifetime = ros::Duration();
marker_pub->publish(marker);
// TODO: subtract out odometry position, and publish transform from
// map to odom
double tf_x = marker.pose.position.x;
double tf_y = marker.pose.position.y;
double tf_yaw = bearing;
// publish a transform based on the position
if( use_odom ) {
// if we're using odometry, look up the odom transform and subtract it
// from our position so that we can publish a map->odom transform
// such that map->odom->base_link reports the correct position
std::string tf_err;
if( tf_buffer.canTransform(pose_frame, odom_frame, now,
ros::Duration(0.1), &tf_err ) ) {
// get odometry position from TF
tf2::Quaternion tf_quat;
tf_quat.setRPY(0.0, 0.0, tf_yaw);
tf2::Transform pose(tf_quat, tf2::Vector3(tf_x, tf_y, 0));
// look up camera transform if we can
if( last_camera_frame.length() > 0 ) {
if( tf_buffer.canTransform(pose_frame, last_camera_frame, now,
ros::Duration(0.1), &tf_err) ) {
geometry_msgs::TransformStamped camera_tf;
camera_tf = tf_buffer.lookupTransform(pose_frame,
last_camera_frame, now);
tf2::Transform camera = msg_to_tf(camera_tf);
pose = pose * camera.inverse();
} else {
ROS_ERROR("Cannot look up transform from %s to %s: %s",
pose_frame.c_str(), last_camera_frame.c_str(), tf_err.c_str());
}
}
geometry_msgs::TransformStamped odom;
odom = tf_buffer.lookupTransform(odom_frame, pose_frame, now);
tf2::Transform odom_tf = msg_to_tf(odom);
// M = C * O
// C^-1 * M = O
// C^-1 = O * M-1
tf2::Transform odom_correction = (odom_tf * pose.inverse()).inverse();
geometry_msgs::TransformStamped transform;
tf2::Vector3 odom_correction_v = odom_correction.getOrigin();
transform.transform.translation.x = odom_correction_v.getX();
transform.transform.translation.y = odom_correction_v.getY();
transform.transform.translation.z = odom_correction_v.getZ();
tf2::Quaternion odom_correction_q = odom_correction.getRotation();
transform.transform.rotation.x = odom_correction_q.getX();
transform.transform.rotation.y = odom_correction_q.getY();
transform.transform.rotation.z = odom_correction_q.getZ();
transform.transform.rotation.w = odom_correction_q.getW();
transform.header.stamp = now;
transform.header.frame_id = world_frame;
transform.child_frame_id = odom_frame;
//tf2::transformTF2ToMsg(odom_correction, transform, now, world_frame,
//odom_frame);
if (publish_tf)
tf_pub.sendTransform(transform);
} else {
ROS_ERROR("Can't look up base transform from %s to %s: %s",
pose_frame.c_str(),
odom_frame.c_str(),
tf_err.c_str());
}
} else {
// we're publishing absolute position
geometry_msgs::TransformStamped transform;
transform.header.stamp = now;
transform.header.frame_id = world_frame;
transform.child_frame_id = pose_frame;
transform.transform.translation.x = tf_x;
transform.transform.translation.y = tf_y;
transform.transform.translation.z = 0.0;
transform.transform.rotation = tf::createQuaternionMsgFromYaw(tf_yaw);
if (publish_tf)
tf_pub.sendTransform(transform);
}
}
