Marker* readMarkers(const char* path)
{
Marker* head;
char name[20];
double ratio;
FILE* file;
file = fopen(path, "r");
if (!file)
{
fprintf(stderr, "Failed to open file (%s).\n", path);
exit(EXIT_FAILURE);
}
head = NULL;
while (fscanf(file, "%19s%lf", name, &ratio) != EOF)
{
fgetc(file); // Skip '\n'
head = insertMarker(head, makeMarker(strCopy(name), ratio));
}
fclose(file);
return head;
}
Block* makeExitSlow(IRGS& env) {
auto const exit = env.unit.defBlock(Block::Hint::Unlikely);
BlockPusher bp(*env.irb, makeMarker(env, bcOff(env)), exit);
interpOne(env, *env.currentNormalizedInstruction);
// If it changes the PC, InterpOneCF will get us to the new location.
if (!opcodeChangesPC(env.currentNormalizedInstruction->op())) {
gen(env, Jmp, makeExit(env, nextBcOff(env)));
}
return exit;
}
visualization_msgs::MarkerArray
makeArray(const ragnar_kinematics::IntermediatePoints& pts)
{
visualization_msgs::MarkerArray array;
array.markers.push_back(makeMarker(pts.A.col(0),
pts.B.col(0),
pts.C.col(0),
"0"));
array.markers.push_back(makeMarker(pts.A.col(1),
pts.B.col(1),
pts.C.col(1),
"1"));
array.markers.push_back(makeMarker(pts.A.col(2),
pts.B.col(2),
pts.C.col(2),
"2"));
array.markers.push_back(makeMarker(pts.A.col(3),
pts.B.col(3),
pts.C.col(3),
"3"));
return array;
}
Block* makeExitOpt(HTS& env, TransID transId) {
assert(!isInlining(env));
auto const targetBcOff = bcOff(env);
auto const exit = env.unit.defBlock(Block::Hint::Unlikely);
BlockPusher blockPusher(*env.irb, makeMarker(env, targetBcOff), exit);
spillStack(env);
gen(env, AdjustSP, StackOffset { offsetFromSP(env, 0) }, sp(env));
gen(env,
ReqRetranslateOpt,
ReqRetransOptData{transId, SrcKey{curSrcKey(env), targetBcOff}},
sp(env));
return exit;
}
Block* makeExitOpt(IRGS& env, TransID transId) {
assertx(!isInlining(env));
auto const targetBcOff = bcOff(env);
auto const exit = defBlock(env, Block::Hint::Unlikely);
BlockPusher blockPusher(*env.irb, makeMarker(env, targetBcOff), exit);
auto const data = ReqRetranslateOptData {
transId,
SrcKey { curSrcKey(env), targetBcOff },
bcSPOffset(env)
};
gen(env, ReqRetranslateOpt, data, sp(env), fp(env));
return exit;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "rviz_logo_marker");
ros::NodeHandle n;
tf::TransformBroadcaster tf_broadcaster;
ros::Timer publish_timer = n.createTimer(ros::Duration(0.1), boost::bind(&publishCallback,tf_broadcaster,_1));
server = new interactive_markers::InteractiveMarkerServer("rviz_logo");
makeMarker( );
ros::spin();
}
void callback(const gazebo_msgs::ModelStates::ConstPtr& msg)
{
// ModelStates does not have a timestamp: work around: use own time!!
//throttle to about 20 Hz as Gazebo publishes at 1000Hz
ros::Time time_now = ros::Time::now();
static ros::Time time_last = time_now;
ros::Duration dur_last = time_now - time_last;
if (dur_last >= sleep_dur)
{
tf::Transform obj_t;
static tf::Transform obj_t_old;
tf::Transform rob;
float vel_now;
static float vel_old;
float acc_now;
static float acc_old;
float jerk_now;
visualization_msgs::MarkerArray marker_array;
std::vector<tf::Transform> poses = getObjectPose(msg, object_name);
rob = poses.at(0);
obj_t = poses.at(1);
//ROS_INFO("object origin: %4.2f,%4.2f",obj_t.getOrigin().getX(), obj_t.getOrigin().getY());
//Object frame and mesh
tf::StampedTransform tr(
//gaz2map_ * obj_t,
obj_t,
//rob.inverse() * obj_t * rob2try.inverse(),
time_now,
//std::string("/map"),
std::string("/gazebo_link"),
//std::string("/base_tray_link"),
std::string("/tray_object_1"));
br.sendTransform(tr);
marker_array.markers.push_back(makeMarker(tr, std::string("object_model")));
// Reference angle
tf::Quaternion ref_ang(0,0,0,1);
//ROS_INFO("Z axis: %4.2f, %4.2f, %4.2f",/*{{{*/
// tf::Matrix3x3(ref_ang).getColumn(2).getX(),
// tf::Matrix3x3(ref_ang).getColumn(2).getY(),
// tf::Matrix3x3(ref_ang).getColumn(2).getZ());/*}}}*/
ref_ang.setRPY(0,-M_PI/2,0);
tf::Transform obj_ref =
gaz2map_ * tf::Transform(ref_ang, obj_t.getOrigin());
marker_array.markers.push_back(makeMarker(tf::StampedTransform(
obj_ref,
time_now,
std::string("/map"),
std::string("/tray_object")), std::string("ref_angle"), true));
// actual object angle
tf::Transform obj_ang =
gaz2map_ * obj_t;// * tf::Transform(ref_ang, tf::Vector3(0,0,0));
tf::Vector3 z_ax_obj(
tf::Matrix3x3(obj_ang.getRotation()).getColumn(2).getX(),
tf::Matrix3x3(obj_ang.getRotation()).getColumn(2).getY(),
tf::Matrix3x3(obj_ang.getRotation()).getColumn(2).getZ());
//ROS_INFO("Z axis obj: %4.2f, %4.2f, %4.2f",/*{{{*/
// z_ax_obj.getX(),
// z_ax_obj.getY(),
// z_ax_obj.getZ());/*}}}*/
marker_array.markers.push_back(makeMarker(tf::StampedTransform(
obj_ang * tf::Transform(ref_ang, tf::Vector3(0,0,0)),
time_now,
std::string("/map"),
std::string("/tray_object")), std::string("obj_angle"), true));
// publish the marker
mark_pub->publish(marker_array);
//Angle between reference vector and object vector (z)
double ang =
acos(
z_ax_obj.getZ() / sqrt(
+z_ax_obj.getX()*z_ax_obj.getX()
+z_ax_obj.getY()*z_ax_obj.getY()
+z_ax_obj.getZ()*z_ax_obj.getZ()));
//ROS_INFO("Angle: %4.2f", ang);
geometry_msgs::Vector3Stamped dev_msg;
dev_msg.header.stamp = time_now;
dev_msg.header.frame_id = "/base_footprint";
dev_msg.vector.x = ang;
//relative translatory deviation object to tray
tf::Vector3 tray_obj = obj_t.getOrigin() - (rob*rob2obj).getOrigin();
//vector length is trans deviation
dev_msg.vector.y = sqrt(
+tray_obj.getX()*tray_obj.getX()
+tray_obj.getY()*tray_obj.getY()
+tray_obj.getZ()*tray_obj.getZ());
dev_pub->publish(dev_msg);
// object dynamcis
vel_now = getVel(obj_t.getOrigin(), obj_t_old.getOrigin(), dur_last);
vel_now = getMovingAverage(vel_now);
acc_now = getDerivation(vel_now, vel_old, dur_last);
jerk_now = getDerivation(acc_now, acc_old, dur_last);
float orient = getOrient(obj_t, obj_t_old);
//ROS_INFO("Obj. dynamics: %4.2f,%4.2f,%4.2f,%4.2f", vel_now, acc_now, jerk_now, orient);
std::vector<float> dyn = {vel_now, acc_now, jerk_now, orient};
publishDynamics(dyn, time_now);
obj_t_old = obj_t;
vel_old = vel_now;
acc_old = acc_now;
time_last = time_now;
}
}
