bool BaxterCommander::pick()
{
RobotMoveit::WhichArm arm = RobotMoveit::RIGHT;
Gripper::WhichArm gripper = Gripper::RIGHT;
/* Get object location */
if(!getObjectPose())
{
return false;
}
baxter_gripper.open(gripper);
/* Execute Cartesian goals */
if(!baxter_movit.executeCarteGoal(arm, object_location))
{
return false;
}
baxter_gripper.close(gripper);
return true;
}
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;
}
}
