/**********************************************************************
* make_edgept
*
* Create an EDGEPT and hook it into an existing list of edge points.
**********************************************************************/
EDGEPT *make_edgept(int x, int y, EDGEPT *next, EDGEPT *prev) {
EDGEPT *this_edgept;
/* Create point */
this_edgept = new EDGEPT;
this_edgept->pos.x = x;
this_edgept->pos.y = y;
// Now deal with the src_outline steps.
C_OUTLINE* prev_ol = prev->src_outline;
if (prev_ol != NULL && prev->next == next) {
// Compute the fraction of the segment that is being cut.
FCOORD segment_vec(next->pos.x - prev->pos.x, next->pos.y - prev->pos.y);
FCOORD target_vec(x - prev->pos.x, y - prev->pos.y);
double cut_fraction = target_vec.length() / segment_vec.length();
// Get the start and end at the step level.
ICOORD step_start = prev_ol->position_at_index(prev->start_step);
int end_step = prev->start_step + prev->step_count;
int step_length = prev_ol->pathlength();
ICOORD step_end = prev_ol->position_at_index(end_step % step_length);
ICOORD step_vec = step_end - step_start;
double target_length = step_vec.length() * cut_fraction;
// Find the point on the segment that gives the length nearest to target.
int best_step = prev->start_step;
ICOORD total_step(0, 0);
double best_dist = target_length;
for (int s = prev->start_step; s < end_step; ++s) {
total_step += prev_ol->step(s % step_length);
double dist = fabs(target_length - total_step.length());
if (dist < best_dist) {
best_dist = dist;
best_step = s + 1;
}
}
// The new point is an intermediate point.
this_edgept->src_outline = prev_ol;
this_edgept->step_count = end_step - best_step;
this_edgept->start_step = best_step % step_length;
prev->step_count = best_step - prev->start_step;
} else {
// The new point is poly only.
this_edgept->src_outline = NULL;
this_edgept->step_count = 0;
this_edgept->start_step = 0;
}
/* Hook it up */
this_edgept->next = next;
this_edgept->prev = prev;
prev->next = this_edgept;
next->prev = this_edgept;
/* Set up vec entries */
this_edgept->vec.x = this_edgept->next->pos.x - x;
this_edgept->vec.y = this_edgept->next->pos.y - y;
this_edgept->prev->vec.x = x - this_edgept->prev->pos.x;
this_edgept->prev->vec.y = y - this_edgept->prev->pos.y;
return this_edgept;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "master_gamepad"); // remember he-man? awesome.
ros::NodeHandle n;
ros::Publisher tf_pub = n.advertise<geometry_msgs::Transform>("target_frame", 1);
ros::Subscriber joy_sub = n.subscribe("joy", 1, joy_cb);
ros::ServiceClient ik_params_client = n.serviceClient<openarms::ArmIKParams>("arm_ik_params");
g_target_pub = &tf_pub;
for (int i = 0; i < 4; i++)
g_joy_axes[i] = 0;
for (int i = 0; i < MAX_BUTTONS; i++)
g_joy_buttons[i] = 0;
puts("greetings. ctrl-c to exit.");
//t = tf::Transform(btQuaternion::getIdentity(), btVector3(0,0,0));
ros::Rate loop_rate(50);
geometry_msgs::Transform tf_msg;
//btVector3 target_vec(0, 0.75, -0.2);
btVector3 target_vec(0.63, -0.12, -0.17); //0, 0.75, -0.2);
tf::TransformBroadcaster tf_broadcaster;
//btQuaternion orient(btQuaternion(btVector3(1, 0, 0), -2.5)); // *
//btQuaternion(btVector3(1, 0, 0), 2.50));
btQuaternion orient(-0.092, 0.683, -0.724, -0.019); // 0.615, -0.755, 0.166, -0.152);
int nullspace_move = 0;
openarms::ArmIKParams ik_params;
while (ros::ok())
{
if (!g_joy_buttons[5])
{
double speed = (g_joy_buttons[4] ? 0.005 : 0.001);
target_vec += speed * btVector3(-g_joy_axes[0],
g_joy_axes[1],
g_joy_axes[3]);
}
else
{
double speed = 0.01;
if (g_joy_buttons[4])
speed = 0.04;
else if (g_joy_buttons[6])
speed = 0.10;
orient = btQuaternion(btVector3(1, 0, 0), speed * g_joy_axes[1]) * orient;
orient = btQuaternion(btVector3(0, 0, 1), speed * g_joy_axes[0]) * orient;
orient = orient * btQuaternion(btVector3(0, 0, 1), speed * g_joy_axes[2]);
orient.normalize();
}
if (g_joy_buttons[1] && nullspace_move != -1)
{
nullspace_move = -1;
ik_params.request.posture = 0.95;
ik_params.request.posture_gain = 0.5;
ik_params_client.call(ik_params);
}
else if (g_joy_buttons[2] && nullspace_move != 1)
{
nullspace_move = 1;
ik_params.request.posture = 0.05;
ik_params.request.posture_gain = 0.5;
ik_params_client.call(ik_params);
}
else if (g_joy_buttons[0] && nullspace_move != -2)
{
nullspace_move = -2;
ik_params.request.posture = 0.95;
ik_params.request.posture_gain = 2.0;
ik_params_client.call(ik_params);
}
else if (g_joy_buttons[3] && nullspace_move != 2)
{
nullspace_move = 2;
ik_params.request.posture = 0.05;
ik_params.request.posture_gain = 2.0;
ik_params_client.call(ik_params);
}
else if (!g_joy_buttons[1] && !g_joy_buttons[2] && nullspace_move != 0)
{
nullspace_move = 0;
ik_params.request.posture = 0.3;
ik_params.request.posture_gain = 0;
ik_params_client.call(ik_params);
}
/*
tf::transformTFToMsg(tf::Transform(btQuaternion::getIdentity(), target_vec),
tf_msg);
*/
tf::StampedTransform t_target_in_world(tf::Transform(orient, target_vec),
ros::Time::now(),
"world", "ik_target");
geometry_msgs::TransformStamped t_target_msg;
tf::transformStampedTFToMsg(t_target_in_world, t_target_msg);
tf_broadcaster.sendTransform(t_target_msg);
//g_target_pub->publish(tf_msg);
loop_rate.sleep();
ros::spinOnce();
}
printf("bai\n");
return 0;
}
