Ejemplo n.º 1
0
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;
}
Ejemplo n.º 2
0
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;
}
Ejemplo n.º 3
0
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;
}
Ejemplo n.º 4
0
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;
}
Ejemplo n.º 5
0
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;
}
Ejemplo n.º 6
0
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();
}
Ejemplo n.º 7
0
    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;
      }
    }