//Pose is global pose with odometry
void cob_cartesian_trajectories::cartStateCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
if(bRun)
{
ros::Duration dt = ros::Time::now() - timer;
cout << "\n" << "===================================" << "\n\n" << "dt: " << dt << "\n";
timer = ros::Time::now();
if((targetDuration-currentDuration) <= 0)
{
geometry_msgs::Twist twist;
cart_command_pub.publish(twist);
ROS_INFO("finished trajectory in %f", ros::Time::now().toSec() - tstart.toSec());
success = true;
stopTrajectory();
return;
}
KDL::Frame current;
KDL::Frame myhinge;
tf::PoseMsgToKDL(msg->pose, current);
tf::PoseMsgToKDL(current_hinge.pose, myhinge);
KDL::Vector unitz = myhinge.M.UnitZ();
std::cout << "Radius because of Hinge: " << (myhinge.p - current.p) << "UnitZ of hinge: " << unitz.z() << "\n";
// get twist to be published
geometry_msgs::Twist twist;
twist = getTwist(currentDuration, current);
// publish twist
cart_command_pub.publish(twist);
// add needed time
currentDuration+=dt.toSec();
// add position to be visualized in rviz
geometry_msgs::Point p;
p.x = msg->pose.position.x;
p.y = msg->pose.position.y;
p.z = msg->pose.position.z;
trajectory_points.push_back(p);
}
else
{
//publish zero
geometry_msgs::Twist twist;
cart_command_pub.publish(twist);
}
}
void Camera::dragMouse( int x, int y )
{
Vec3f mouseDelta = Vec3f(x,y,0.0f) - mLastMousePosition;
mLastMousePosition = Vec3f(x,y,0.0f);
switch(mCurrentMouseAction)
{
case kActionTranslate:
{
calculateViewingTransformParameters();
double xTrack = -mouseDelta[0] * kMouseTranslationXSensitivity;
double yTrack = mouseDelta[1] * kMouseTranslationYSensitivity;
Vec3f transXAxis = mUpVector ^ (mPosition - mLookAt);
transXAxis /= sqrt((transXAxis*transXAxis));
Vec3f transYAxis = (mPosition - mLookAt) ^ transXAxis;
transYAxis /= sqrt((transYAxis*transYAxis));
setLookAt(getLookAt() + transXAxis*xTrack + transYAxis*yTrack);
break;
}
case kActionRotate:
{
float dAzimuth = -mouseDelta[0] * kMouseRotationSensitivity;
float dElevation = mouseDelta[1] * kMouseRotationSensitivity;
setAzimuth(getAzimuth() + dAzimuth);
setElevation(getElevation() + dElevation);
break;
}
case kActionZoom:
{
float dDolly = -mouseDelta[1] * kMouseZoomSensitivity;
float dTwist = mouseDelta[0] * kMouseTwistSensitivity;
setDolly(getDolly() + dDolly);
setTwist(getTwist() + dTwist);
break;
}
default:
break;
}
}
