/* Get the nearest known position we know the line number of
(i.e. BUF_BEGV, and cached positions). The return position will be
either closer than BEG, or BEG. The line of this known position
will be stored in LINE.
*LINE should be initialized to the line number of BEG (normally,
BEG will be BUF_BEGV, and *LINE will be XFIXNUM (LINE_NUMBER_BEGV).
This will initialize the cache, if necessary. */
static void
get_nearest_line_number (struct buffer *b, Charbpos *beg, Charbpos pos,
EMACS_INT *line)
{
EMACS_INT i;
Lisp_Object *ring = XVECTOR_DATA (LINE_NUMBER_RING (b));
Charcount length = pos - *beg;
if (length < 0)
length = -length;
/* Find the ring entry closest to POS, if it is closer than BEG. */
for (i = 0; i < LINE_NUMBER_RING_SIZE && CONSP (ring[i]); i++)
{
Charbpos newpos = marker_position (XCAR (ring[i]));
Charcount howfar = newpos - pos;
if (howfar < 0)
howfar = -howfar;
if (howfar < length)
{
length = howfar;
*beg = newpos;
*line = XFIXNUM (XCDR (ring[i]));
}
}
}
/* Invalidate the line number cache positions that lie after POS. */
static void
invalidate_line_number_cache (struct buffer *b, Charbpos pos)
{
EMACS_INT i, j;
Lisp_Object *ring = XVECTOR_DATA (LINE_NUMBER_RING (b));
for (i = 0; i < LINE_NUMBER_RING_SIZE; i++)
{
if (!CONSP (ring[i]))
break;
/* As the marker stays behind the insertions, this check might
as well be `>'. However, Finsert_before_markers can advance
the marker anyway, which bites in shell buffers.
#### This forces recreation of the cached marker (and
recalculation of newlines) every time a newline is inserted
at point, which is way losing. Isn't there a way to make a
marker impervious to Finsert_before_markers()?? Maybe I
should convert the code to use extents. */
if (marker_position (XCAR (ring[i])) >= pos)
{
/* Get the marker out of the way. */
Fset_marker (XCAR (ring[i]), Qnil, Qnil);
/* ...and shift the ring elements, up to the first nil. */
for (j = i; !NILP (ring[j]) && j < LINE_NUMBER_RING_SIZE - 1; j++)
ring[j] = ring[j + 1];
ring[j] = Qnil;
/* Must recheck position i. */
i--;
}
}
}
/* #### The -1 check is such a hack. */
static void
x_update_scrollbar_instance_values (struct window *w,
struct scrollbar_instance *inst,
int new_line_increment,
int new_page_increment,
int new_minimum, int new_maximum,
int new_slider_size,
int new_slider_position,
int new_scrollbar_width,
int new_scrollbar_height,
int new_scrollbar_x, int new_scrollbar_y)
{
UPDATE_DATA_FIELD (line_increment);
UPDATE_DATA_FIELD (page_increment);
UPDATE_DATA_FIELD (minimum);
UPDATE_DATA_FIELD (maximum);
UPDATE_DATA_FIELD (slider_size);
UPDATE_DATA_FIELD (slider_position);
UPDATE_DATA_FIELD (scrollbar_width);
UPDATE_DATA_FIELD (scrollbar_height);
UPDATE_DATA_FIELD (scrollbar_x);
UPDATE_DATA_FIELD (scrollbar_y);
/* This doesn't work with Athena, why? */
#if defined (LWLIB_SCROLLBARS_MOTIF) || defined (LWLIB_SCROLLBARS_LUCID)
if (w && !vertical_drag_in_progress)
{
int new_vov = SCROLLBAR_X_POS_DATA (inst).slider_position;
int new_vows = marker_position (w->start[CURRENT_DISP]);
if (SCROLLBAR_X_VDRAG_ORIG_VALUE (inst) != new_vov)
{
SCROLLBAR_X_VDRAG_ORIG_VALUE (inst) = new_vov;
inst->scrollbar_instance_changed = 1;
}
if (SCROLLBAR_X_VDRAG_ORIG_WINDOW_START (inst) != new_vows)
{
SCROLLBAR_X_VDRAG_ORIG_WINDOW_START (inst) = new_vows;
inst->scrollbar_instance_changed = 1;
}
}
#endif
}
// Publish recorded markers but also push waypoint trajectories.
void pub_recorded_marker(ros::Publisher &marker_pub, visualization_msgs::Marker::ConstPtr &rosbag_marker,
int index, int total_markers, tf::Transform &translate_to_main, tf::Transform &rotate_to_main ){
tf::Vector3 marker_position (rosbag_marker->pose.position.x,
rosbag_marker->pose.position.y,
rosbag_marker->pose.position.z);
tf::Quaternion marker_orientation (rosbag_marker->pose.orientation.x,
rosbag_marker->pose.orientation.y,
rosbag_marker->pose.orientation.z,
rosbag_marker->pose.orientation.w);
// Perform tf transformation.
marker_position = translate_to_main * marker_position;
marker_orientation = rotate_to_main * marker_orientation;
visualization_msgs::Marker marker;
uint32_t shape = visualization_msgs::Marker::CUBE;
// Set the frame ID and timestamp. See the TF tutorials for information on these.
//marker.header.frame_id = "/my_frame";
marker.header.frame_id = "/base_link";
marker.header.stamp = ros::Time::now();
// Set the namespace and id for this marker. This serves to create a unique ID
// Any marker sent with the same namespace and id will overwrite the old one
marker.ns = rosbag_marker->ns;//"basic_shapes";
marker.id = index;//rosbag_marker->id;
// Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
marker.type = rosbag_marker->type; //shape;
// Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
marker.action = visualization_msgs::Marker::ADD;
// Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header
marker.pose.position.x = marker_position.getX();//rosbag_marker->pose.position.x;//0;
marker.pose.position.y = marker_position.getY();//rosbag_marker->pose.position.y;//0;
marker.pose.position.z = marker_position.getZ();//rosbag_marker->pose.position.z;//0;
marker.pose.orientation.x = marker_orientation.getAxis().getX();//rosbag_marker->pose.orientation.x;//0.0;
marker.pose.orientation.y = marker_orientation.getAxis().getY();//rosbag_marker->pose.orientation.y;//0.0;
marker.pose.orientation.z = marker_orientation.getAxis().getZ();//rosbag_marker->pose.orientation.z;//0.0;
marker.pose.orientation.w = marker_orientation.getW();//rosbag_marker->pose.orientation.w; //1.0;
// Set the scale of the marker -- 1x1x1 here means 1m on a side
marker.scale.x = rosbag_marker->scale.x; //1.0;
marker.scale.y = rosbag_marker->scale.y; //1.0;
marker.scale.z = rosbag_marker->scale.z; //0.5;
// Set the color -- be sure to set alpha to something non-zero!
// use index/total_markers percentage as a way to fade from red to green.
marker.color.r = 1.0 * ( (double)(total_markers - index) / (double)total_markers);//rosbag_marker->color.r; //0.0f;
marker.color.g = 1.0f * ( (double)index / (double)total_markers); //1.0f;//rosbag_marker->color.g; //1.0f;
marker.color.b = 0.0f;//rosbag_marker->color.b; //0.0f;
marker.color.a = 1.0f;//1.0 * ( (double)(total_markers - index) / (double)total_markers); //rosbag_marker->color.a; //1.0;
// Make it last forever. We don't need to keep publishing it.
marker.lifetime = ros::Duration();
// Push marker 6DoF Pose to the waypoints vector.
geometry_msgs::Pose target_pose;
target_pose.position.x = marker_position.getX();
target_pose.position.y = marker_position.getY();
target_pose.position.z = marker_position.getZ();
target_pose.orientation.x = marker_orientation.getAxis().getX();//rosbag_marker->pose.orientation.x;//0.0;
target_pose.orientation.y = marker_orientation.getAxis().getY();//rosbag_marker->pose.orientation.y;//0.0;
target_pose.orientation.z = marker_orientation.getAxis().getZ();//rosbag_marker->pose.orientation.z;//0.0;
target_pose.orientation.w = marker_orientation.getW();//rosbag_marker->pose.orientation.w; //1.0;
waypoints.push_back(target_pose);
// Publish the marker
while (marker_pub.getNumSubscribers() < 1)
{
if (!ros::ok())
{
ROS_WARN_ONCE("NOT OK!");
break;
}
ROS_WARN_ONCE("Please create a subscriber to the marker");
sleep(1.0);
}
marker_pub.publish(marker);
}