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