void read_timerCallback(const ros::TimerEvent &e)
{
std_msgs::String msg_display;
std::stringstream ss_display;
std_msgs::Float64 msg;
// control the motor and publish the real actual position
ROS_INFO("startread");
readActualPosition(&pos);
ROS_INFO("endread");
angle = (double)(pos-origin_pos)*angle_ratio;
transform.setOrigin( tf::Vector3(0, -ORIGIN_OFFSET*sin(angle), -ORIGIN_OFFSET*cos(angle)) );
q.setRPY(-angle, 0, 0);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "laser"));
// printf("\nEPOS position: %ld.\n", pos);
if(pos==origin_pos+ROTATION_ANGLE)
moveAbsolute(origin_pos-ROTATION_ANGLE);
else if(pos==origin_pos-ROTATION_ANGLE)
moveAbsolute(origin_pos+ROTATION_ANGLE);
ss_display << "EPOS position: " << pos;
msg_display.data = ss_display.str();
// %EndTag(FILL_MESSAGE)%
// %Tag(ROSCONSOLE)%
ROS_INFO("%s", msg_display.data.c_str());
msg.data = pos;
// %EndTag(ROSCONSOLE)%
/**
* The publish() function is how you send messages. The parameter
* is the message object. The type of this object must agree with the type
* given as a template parameter to the advertise<>() call, as was done
* in the constructor above.
*/
// %Tag(PUBLISH)%
motorangle_pub.publish(msg);
}
// Kamarabewegung aktualisieren und auf Eingabe, die die Kamera beeinflusst, reagieren
void CameraController::update ( float deltaTime ) // time_span in seconds
{
// Falls die Kamera mitten in einer Bewegung ist, muss sie etwas gegen den Zielort verschoben werden
if ( m_isMovingSmoothly )
{
m_moveTimeElapsed += deltaTime; // die verstrichene Zeit seit Bewegungsanfang wird erhöht
float t = m_moveTimeElapsed/m_moveTotalTimeToArrive; // t ist ein Skalar und gibt den Bruchteil
// des Weges an (oder der Zeit), den wir schon erreicht haben
// t geht von 0 (Start) bis zu 1 (Ziel), falls wir schon über 1 sind, dann ist das Ziel schon erreicht.
if ( t > 1.0f )
{
// Ziel erreicht
m_isMovingSmoothly = false;
t = 1.0;
}
// t [0-1] ist gleichmässig auf die Kamerastrecke verteillt (linear). D.h. konstante Geschwindigket:
//
// s | o Ziel
// | /
// | / s = erreichte Strecke der Kamera
// | / t = Bruchteil der Gesamtzeit
// |/
// Start o-----| t
// 0 1
//
// Wir wollen aber eine ruhige Kamerafahrt, d.h. zuerst eine Beschleunigung und dann eine Abbremsung.
// Doch anstatt mit aufwendigen Geschwindigkeiten und Beschleunigungen zu rechnen, verzerren wir einfach die t Variable,
// so dass die oben gezeichnete Linie zu einer 'S'-Kurve wird, die am anfang wenig steil ist, bei t=0.5 am steilsten ist und
// dann wieder abflacht:
//
// s | o Ziel
// | x
// | x
// | x s = erreichte Strecke der Kamera
// | x t = Bruchteil der Gesamtzeit
// | x
// | x
// | x
// Start o--------------------| t
// 0 0.5 1
//
// Die este Häfte der Kurve ist wie der Anfang einer einfachen exponentiellen Kurve (x^2) (von 0 bis 1).
// Damit wir also t richtig hinbekommen, machen wir die este Häfte zu eine x^2 kurve.
// In der ersten Hälfte geht t von 0 bis 0.5. Wir müssen t also provisorisch verdoppeln damit wir eine [0 bis 1] Kurve haben.
// Dann ist die kurve jedoch doppelt so gross, deshalb noch durch 2 teilen. Am schluss:
// Wenn t<0.5, dann wird t zu (t*2)^2/2 = 2*t^2
//
// In der zweiten Hälfte der Kurve gleich verzerren, jedoch dann die ergänzung zu 1 nehmen. (es ist wie eine gespiegelte x^2 Kurve)
// Wir haben aber Zahlen über 0.5 und wollen sie wie vorher behandeln, also zuerst 1-t rechnen, dann sind sie wie in der erste Hälfte.
// Dann das Gleiche tun wie vorher (2*t^2).
// Dann die Ergänzung zu 1 rechnen (wieder 1-t). Am schluss gibt es:
// Wenn t>0.5, dann wird t zu (1-t) dann zu (2*t^2) dann zu (1-t).
//
// Bei t=0.5 bleibt es gleich (ist Zentrum der Verzerrung).
if ( t < 0.5 )
t = t*t*2;
if ( t > 0.5 )
{
t = 1-t;
t = t*t*2;
t = 1-t;
}
// Neue Position der Kamera berechnen
// Geradengleichung: r = a + t*b | a = Start, b = Zielrichtung, t = Skalar
Vector2D new_pos = m_cameraDeparture + ( m_cameraDestination-m_cameraDeparture ) *t;
moveAbsolute( new_pos, 0.0f ); // Kameraposition ein wenig gegen richtung Ziel verschieben
}
if ( m_isRotatingSmoothly )
{
m_rotateTimeElapsed += deltaTime; // die verstrichene Zeit seit Rotationsanfang wird erhöht
float t = m_rotateTimeElapsed/m_rotateTotalTimeToArrive; // t ist ein Skalar und gibt den Bruchteil
// der Rotation an (oder der Zeit), die wir schon erreicht haben
// t geht von 0 (Start) bis zu 1 (Ziel), falls wir schon über 1 sind, dann ist das Ziel schon erreicht.
if ( t > 1.0f )
{
// Ziel erreicht
m_isRotatingSmoothly = false;
t = 1.0;
}
if ( t < 0.5 )
t = t*t*2; // Wie bei der Translation der Kamara, siehe kommentar oben.
if ( t > 0.5 )
{
t = 1-t;
t = t*t*2;
t = 1-t;
}
// Neue Rotation der Kamera berechnen
// r = s + t*e | s = Startwinkel, z = Zielwinkel, t = Skalar
float new_angle = m_startingAngle + ( m_endingAngle-m_startingAngle ) *t;
rotateAbsolute ( new_angle, 0.0f ); // Kameraposition ein wenig gegen Zielwinkel drehen
}
}
// EPOS motor initialize
int EPOS_initialize(long &pos, long &origin_pos){
char name[128];
int n;
WORD w = 0x0, estatus=0x0;
// WORD dw[2] = {0x0, 0x0};
printf("\n *** EPOS Motor Control ***\n\n");
printf("\n an alternating '/', '\' char means that the program is waiting\n");
printf(" for the EPOS to respond.\n");
/* open EPOS device */
if ( openEPOS("/dev/ttyUSB2") < 0) exit(-1);
/* read manufactor device name */
if ( readDeviceName(name) < 0) {
fprintf(stderr, "ERROR: cannot read device name!\n");
}
else {
printf("\ndevice name is: %s\n", name);
}
// ask for software version (get HEX answer!!!)
printf("\nsoftware version: %x\n", readSWversion() );
// RS232 timeout (14.1.35)
if ( ( n = readRS232timeout() ) < 0) checkEPOSerror();
else printf("\nRS232 timeout is: %d ms\n", n );
// EPOS Status
estatus = 0x0;
if ( ( n = readStatusword(&estatus) ) < 0) checkEPOSerror();
else printf("\nEPOS status is: %#06x \n", estatus );
//printEPOSstatusword(estatus);
//printEPOSstate();
/******************************************************
switch on
******************************************************/
// 1. check if we are in FAULT state
n = checkEPOSstate();
if ( n == 11) {
printf("EPOS is in FAULT state, doing FAULT RESET ");
// should check which fault this is, assume it's a CAN error from
// power-on
// 1a. reset FAULT
changeEPOSstate(6);
// 1b. check status
if ( checkEPOSstate() == 11 ){
fprintf(stderr, "\nEPOS still in FAULT state, quit!\n");
exit(1);
}
else printf("success!\n");
}
else if (n != 4 && n!= 7) { // EPOS not running, issue a quick stop
//printf("\nsending QUICK STOP\n");
changeEPOSstate(3);
// 2. we should now be in 'switch on disabled' (2)
n = checkEPOSstate();
if ( n != 2 ){
printf("EPOS is NOT in 'switch on disabled' state, quit!\n");
printf("(%s(), %s line %d)\n", __func__, __FILE__, __LINE__);
exit(1);
}
else { // EPOS is in 'switch on disabled'
//printf("EPOS is in 'switch on disabled' state, doing shutdown. \n");
changeEPOSstate(0); // issue a 'shutdown'
//printEPOSstate();
}
// 3. switch on
printf("\nswitching on ");
changeEPOSstate(1);
//printEPOSstate();
// 4. enable operation
printf("\nenable operation " );
changeEPOSstate(5);
}
//printEPOSstate();
// position window
unsigned long win;
readPositionWindow(&win);
printf("\nEPOS position window is %lu. ", win);
if (win == 4294967295 ) printf("-> position window is switched off!\n");
else printf("\n");
// actual position
readActualPosition(&pos);
printf("\nEPOS position SHOULD be %ld. Starting... \n", pos);
origin_pos = pos;
// check, if we are in Profile Position Mode
// if (readOpMode() != E_PROFPOS) {
if( setOpMode(E_PROFPOS) ) {
fprintf(stderr, "ERROR: problem at %s; %s line %d\n",
__func__, __FILE__, __LINE__);
return(-1);
}
// }
moveAbsolute(origin_pos+ROTATION_ANGLE);
// int sum = 0;
return 0;
}
