void callBack(const my_ros::control_param::ConstPtr& msg) {
if(0==flag_move)
{
setTwist();
return;
}
float weigh_w[5] = { 0.56, 0.28, 0.1, 0.04, 0.02 };
ROS_INFO("dist: %f theta1: %f theta2: %f RADIUS: %f ", msg->dist,
msg->theta1, msg->theta2, RADIUS);
float d = msg->dist; //dist>0:left ; dist<0:right;
float w = -msg->theta1; //theat>0:conterclockwise ; theta<0:clockwise
w /= (RADIUS * 20);
if (w > 1.047)
w = 1.047;
if (w < -1.047)
w = -1.047;
list_w->push_front(w);
list_w->pop_back();
int ind = 0;
w = 0;
for (std::list<float>::iterator it = list_w->begin();
it != list_w->end(); it++, ind++) {
w += (*it) * weigh_w[ind];
//std::cout<<(*it)*weigh_w[ind]<<std::endl;
//ROS_INFO("w :%f :", w);
}
setTwist(0.1,0,0,0,0,w);
ROS_INFO("linear :%f , angular: %f",twist_msg.linear.x,twist_msg.angular.z);
}
~mycontroller() {
flag_move=0;
keyop_='s';
delete list_w;
tcsetattr(kfd, TCSANOW, &raw);
thread.cancel();
thread.join();
setTwist();
pub_.publish(twist_msg);
}
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;
}
}