int main(int argc, char** argv)
{
ros::init(argc, argv, "set_tfs");
ros::NodeHandle n;
ros::Subscriber pose_sub = n.subscribe<gazebo_msgs::ModelStates>("gazebo/model_states", 10, poseCallback);
static tf::TransformBroadcaster roomba_base1;
tf::Transform transform1;
static tf::TransformBroadcaster roomba_base2;
tf::Transform transform2;
ros::Rate r(1000);
while(ros::ok()){
transform1.setOrigin( tf::Vector3(roomba_pose1.position.x, roomba_pose1.position.y, roomba_pose1.position.z ));
transform1.setRotation( tf::Quaternion(roomba_pose1.orientation.x, roomba_pose1.orientation.y, roomba_pose1.orientation.z, roomba_pose1.orientation.w));
transform2.setOrigin( tf::Vector3(roomba_pose2.position.x, roomba_pose2.position.y, roomba_pose2.position.z ));
transform2.setRotation( tf::Quaternion(roomba_pose2.orientation.x, roomba_pose2.orientation.y, roomba_pose2.orientation.z, roomba_pose2.orientation.w));
roomba_base1.sendTransform(tf::StampedTransform(transform1, ros::Time::now(), "odom", "roomba_base1"));
roomba_base2.sendTransform(tf::StampedTransform(transform2, ros::Time::now(), "odom", "roomba_base2"));
ros::spinOnce();
r.sleep();
}
return 0;
}
/* This is just for reference
void poseCallback(const turtlesim::PoseConstPtr& msg){
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
*/
int main(int argc, char** argv){
ros::init(argc, argv, "kinect_broadcaster");
ros::NodeHandle node;
ROS_INFO("Started kinect_broadcaster node");
ros::Rate rate(10.0);
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin(tf::Vector3(0,0,0));
tf::Quaternion q;
q.setRPY(0,0,0);
transform.setRotation(q);
static tf::TransformBroadcaster world_openni;
tf::Transform transform2;
transform2.setOrigin(tf::Vector3(0,0,0));
tf::Quaternion q2;
q2.setRPY(0,0,0);
transform2.setRotation(q);
while (node.ok()){
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(),"world","camera_link"));
world_openni.sendTransform(tf::StampedTransform(transform2,
ros::Time::now(),"world","openni_depth_frame"));
rate.sleep();
}
return 0;
};
void PublishTransform(ros::Time stamp, float fX, float fY, float fZ, float fYaw, float fPitch, float fRoll)
{
static tf::TransformBroadcaster tfBroadcaster;
static tf::Transform transform;
//from world to vehile;
transform.setOrigin(tf::Vector3(fX, fY, fZ));
transform.setRotation(tf::Quaternion(fYaw, fPitch, fRoll));
tfBroadcaster.sendTransform(tf::StampedTransform(transform, stamp, "/world", "/vehicle"));
//from vehile to lms1;
transform.setOrigin(tf::Vector3(1.26, 0.485, 2.196));
//transform.setRotation(tf::Quaternion(0.0125+0.0026+0.0034, 0.183011, 0.0+0.0017*7));//roll, pitch, yaw
transform.setRotation(tf::Quaternion(0.0, 0.183, 0.0));//roll, pitch, yaw
tfBroadcaster.sendTransform(tf::StampedTransform(transform, stamp, "/vehicle", "/lms1"));
//from vehicle to lms2;
transform.setOrigin(tf::Vector3(1.26, -0.467, 2.208));
//transform.setRotation(tf::Quaternion(0.0125003, 0.142386, 6.27694+0.0017*5));
transform.setRotation(tf::Quaternion(0.0, 0.142386, 0.0));
tfBroadcaster.sendTransform(tf::StampedTransform(transform, stamp, "/vehicle", "/lms2"));
//from vehicle to velodyne1;
transform.setOrigin(tf::Vector3(1.147, 0.477, 2.405));
//transform.setRotation(tf::Quaternion(0.0, 0.0017, 0.0)); //yaw, pitch, roll
transform.setRotation(tf::Quaternion(0.0, 0.0, 0.0)); //yaw, pitch, roll
tfBroadcaster.sendTransform(tf::StampedTransform(transform, stamp, "/vehicle", "/velodyne1"));
//from vehicle to velodyne2;
transform.setOrigin(tf::Vector3(1.152, -0.445,2.45));
//transform.setRotation(tf::Quaternion(6.28006,0.000175, 0.0));
transform.setRotation(tf::Quaternion(0.0, 0.0, 0.0));
tfBroadcaster.sendTransform(tf::StampedTransform(transform, stamp, "/vehicle", "/velodyne2"));
}
// this is a function for sending all of the global transforms
// that we want to send:
void send_global_transforms(ros::Time tstamp)
{
tf::Transform transform;
// first publish the optimization frame:
transform.setOrigin(tf::Vector3(cal_pos(0),
cal_pos(1),
cal_pos(2)));
transform.setRotation(tf::Quaternion(0,0,0,1));
br.sendTransform(tf::StampedTransform(transform, tstamp,
"oriented_optimization_frame",
"optimization_frame"));
// publish the map frame at same height as the Kinect
transform.setOrigin(tf::Vector3(cal_pos(0),
0,
cal_pos(2)));
transform.setRotation(tf::Quaternion(.707107,0.0,0.0,-0.707107));
br.sendTransform(tf::StampedTransform(transform, tstamp,
"oriented_optimization_frame",
"map"));
// publish one more frame that is the frame the robot
// calculates its odometry in.
transform.setOrigin(tf::Vector3(0,0,0));
transform.setRotation(tf::Quaternion(1,0,0,0));
br.sendTransform(tf::StampedTransform(transform, tstamp,
"map",
"robot_odom_pov"));
return;
}
bool kinematic_filter::filter(std::list<foot_with_joints> &data)
{
jnt_pos_in=kinematics.lwr_legs.joints_value;
for (auto single_step=data.begin();single_step!=data.end();)
{
auto StanceFoot_MovingFoot=StanceFoot_World*single_step->World_MovingFoot;
if (!frame_is_reachable(StanceFoot_MovingFoot,single_step->joints))
single_step=data.erase(single_step);
else
{
single_step->World_StanceFoot=World_StanceFoot;
KDL::Frame StanceFoot_Waist;
KDL::JntArray temp(kinematics.wl_leg.chain.getNrOfJoints());
for (int i=0;i<temp.rows();i++)
temp(i)=single_step->joints(i);
current_fk_solver->JntToCart(temp,StanceFoot_Waist);
single_step->World_Waist=World_StanceFoot*StanceFoot_Waist;
#ifdef KINEMATICS_OUTPUT
tf::Transform current_robot_transform;
tf::transformKDLToTF(single_step->World_Waist,current_robot_transform);
static tf::TransformBroadcaster br;
br.sendTransform(tf::StampedTransform(current_robot_transform, ros::Time::now(), "world","KNEW_WAIST"));
tf::Transform current_moving_foot_transform;
tf::transformKDLToTF(World_StanceFoot*StanceFoot_MovingFoot,current_moving_foot_transform);
br.sendTransform(tf::StampedTransform(current_moving_foot_transform, ros::Time::now(), "world","Kmoving_foot"));
tf::Transform fucking_transform;
tf::transformKDLToTF(World_StanceFoot,fucking_transform);
br.sendTransform(tf::StampedTransform(fucking_transform, ros::Time::now(), "world", "Kstance_foot"));
#endif
single_step++;
}
}
return true;
}
void poseCallback(const gazebo_msgs::ModelStates model_states)
{
static tf::TransformBroadcaster br;
int object_index = 0;
for (int i = 0; i < model_states.name.size(); i++) {
if (!(model_states.name[i] == "robot") && !(model_states.name[i] == "ground_plane")) {
object_index = i;
break;
}
}
for (int i = object_index; i < model_states.name.size(); i++) {
tf::Transform transform;
transform.setOrigin(
tf::Vector3(model_states.pose[i].position.x, model_states.pose[i].position.y,
model_states.pose[i].position.z));
tf::Quaternion q;
q.setX(model_states.pose[i].orientation.x);
q.setY(model_states.pose[i].orientation.y);
q.setZ(model_states.pose[i].orientation.z);
q.setW(model_states.pose[i].orientation.w);
transform.setRotation(q);
br.sendTransform(
tf::StampedTransform(transform, ros::Time::now(), "world",
model_states.name[i] + "/base_link"));
br.sendTransform(
tf::StampedTransform(transform, ros::Time::now(), "world",
model_states.name[i] + "/base_link_inertia"));
}
}
// Timer callback
void TRPBViz::timerCallback(const ros::TimerEvent& e) {
tf_start.stamp_ = ros::Time::now();
tf_broadcaster.sendTransform(tf_start);
tf_base_link.stamp_ = ros::Time::now();
tf_broadcaster.sendTransform(tf_base_link);
joint_states.header.stamp = ros::Time::now();
joint_state_pub.publish(joint_states);
}
//Broadcasts the transform from the kinect_rgb_optical frame to the ground frame
//If kinect_rgb_optical has a grandparent (i.e. kinect_link), then a transform from the kinect_link frame to the ground frame is broadcasted such that the given transform is still as specified above
void broadcastKinectTransform(const btTransform& transform, const std::string& kinect_rgb_optical, const std::string& ground, tf::TransformBroadcaster& broadcaster, const tf::TransformListener& listener) {
std::string link;
if (listener.getParent(kinect_rgb_optical, ros::Time(0), link) && listener.getParent(link, ros::Time(0), link)) {
tf::StampedTransform tfTransform;
listener.lookupTransform (link, kinect_rgb_optical, ros::Time(0), tfTransform);
broadcaster.sendTransform(tf::StampedTransform(transform * tfTransform.asBt().inverse(), ros::Time::now(), ground, link));
} else {
broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), ground, kinect_rgb_optical));
}
}
void trCallback(const ros::TimerEvent& event) {
ros::Time now = ros::Time::now() + ros::Duration(0.1);
if (table_calibration_done_) {
tr_table_.stamp_ = now;
br_.sendTransform(tr_table_);
}
if (pr2_calibration_done_) {
tr_pr2_.stamp_ = now;
br_.sendTransform(tr_pr2_);
}
}
void publish_phantom_state()
{
// Construct transforms
tf::Transform l0, sensable;
// Distance from table top to first intersection of the axes
l0.setOrigin(tf::Vector3(0, 0, table_offset_)); // .135 - Omni, .155 - Premium 1.5, .345 - Premium 3.0
l0.setRotation(tf::createQuaternionFromRPY(0, 0, 0));
br_.sendTransform(tf::StampedTransform(l0, ros::Time::now(), base_link_name_.c_str(), link_names_[0].c_str()));
// Displacement from vertical axis towards user.
// Frame in which OpenHaptics report Phantom coordinates. Valid and useful
// for Omni only, since other devices do not do calibration.
sensable.setOrigin(tf::Vector3(-0.2, 0, 0));
sensable.setRotation(tf::createQuaternionFromRPY(M_PI / 2, 0, -M_PI / 2));
br_.sendTransform(
tf::StampedTransform(sensable, ros::Time::now(), link_names_[0].c_str(), sensable_frame_name_.c_str()));
tf::Transform tf_cur_transform;
geometry_msgs::PoseStamped phantom_pose;
// Convert column-major matrix to row-major
tf_cur_transform.setFromOpenGLMatrix(state_->hd_cur_transform);
// Scale from mm to m
tf_cur_transform.setOrigin(tf_cur_transform.getOrigin() / 1000.0);
// Since hd_cur_transform is defined w.r.t. sensable_frame
tf_cur_transform = sensable * tf_cur_transform;
// Rotate end-effector back to base
tf_cur_transform.setRotation(tf_cur_transform.getRotation() * sensable.getRotation().inverse());
// Publish pose in link_0
phantom_pose.header.frame_id = tf::resolve(tf_prefix_, link_names_[0]);
phantom_pose.header.stamp = ros::Time::now();
tf::poseTFToMsg(tf_cur_transform, phantom_pose.pose);
pose_publisher_.publish(phantom_pose);
if ((state_->buttons[0] != state_->buttons_prev[0]) or (state_->buttons[1] != state_->buttons_prev[1]))
{
if ((state_->buttons[0] == state_->buttons[1]) and (state_->buttons[0] == 1))
{
state_->lock = !(state_->lock);
}
sensable_phantom::PhantomButtonEvent button_event;
button_event.grey_button = state_->buttons[0];
button_event.white_button = state_->buttons[1];
state_->buttons_prev[0] = state_->buttons[0];
state_->buttons_prev[1] = state_->buttons[1];
button_publisher_.publish(button_event);
}
}
void PlayerTracker::publishPlayerInfo(int player)
{
player_tracker::PosPrediction pred;
if(player > -1) {
PlayerInfo &it = potentialPlayers[currentPlayer];
std::vector<float> res = it.playerFilter.getStatus();
tf::Transform tr;
tr.setRotation( tf::Quaternion(0,0 , 0, 1) );
tr.setOrigin(tf::Vector3(res[0],res[1],0.0));
transformBroadcaster.sendTransform(tf::StampedTransform(tr, ros::Time::now(), "base_link_respondable", "bill_filtered"));
tr.setOrigin(tf::Vector3(res[2]*2.0,res[3]*2.0,0.0));
transformBroadcaster.sendTransform(tf::StampedTransform(tr, ros::Time::now(), "bill_filtered", "bill_filtered_vel"));
pred.userId = player;
pred.position.x = res[0];
pred.position.y = res[1];
pred.velocity.x = res[2];
pred.velocity.y = res[3];
pred.unreliability = sqrt(res[4] + res[5]);
geometry_msgs::PoseStamped playerPose;
playerPose.header.stamp = ros::Time::now();
playerPose.header.frame_id = "base_footprint";
playerPose.pose.position.x = res[0];
playerPose.pose.position.y = res[1];
playerPose.pose.position.z = 0;
float angle = atan2(res[3],res[2]);
tf::Quaternion orientation(tf::Vector3(0,0,1),angle);
geometry_msgs::Quaternion odom_quat;
tf::quaternionTFToMsg(orientation,odom_quat);
playerPose.pose.orientation = odom_quat;
PosePredictionPublisher.publish(playerPose);
} else {
pred.userId = -1;
pred.position.x = 0;
pred.position.y = 0;
pred.velocity.x = 0;
pred.velocity.y = 0;
pred.unreliability = std::numeric_limits<double>::max();
}
//std::cout <<"unrel: "<<pred.unreliability<<" "<<res[4]<<" "<<res[5]<<std::endl;
predictionPublisher.publish(pred);
}
int main(int argc, char** argv){
// Node initialization
ros::init(argc, argv, "tf_fake");
//ros::Time::init();
ros::NodeHandle node;
if (argc !=1){ROS_ERROR("Error argument");return -1;};
ros::Rate r(10);
// For closed-loop simulation
ros::Subscriber sub = node.subscribe("cmd_vel", 100, drive);
// Tf variable declaration
static tf::TransformBroadcaster br;
tf::Transform transform;
tf::Quaternion q;
while(node.ok())
{
// Data update
x += dx;
y += dy;
yaw_counter += dyaw;
if(yaw_counter > (M_PI/2) && yaw_counter < M_PI){
yaw = yaw_counter - M_PI;
}
else if(yaw_counter > M_PI){
yaw_counter -= M_PI;
yaw = yaw_counter;
}
else{
yaw = yaw_counter;
}
// Data transmit
transform.setOrigin(tf::Vector3(x, y, 0.0));
q.setRPY(0, 0, yaw);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "fake_tf"));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "fake_ls_tf"));
ros::spinOnce();
r.sleep();
};
return 0;
}
void dynamixel_cb(const dynamixel_msgs::JointStateConstPtr& jstate)
{
static tf::TransformBroadcaster br;
tf::Transform transform;
tf::Quaternion q;
transform.setOrigin( tf::Vector3(0., 0., 0.) );
q.setRPY(0, jstate->current_pos, 0);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, jstate->header.stamp, "dynamixel_base", "dynamixel_link"));
transform.setOrigin( tf::Vector3(dtol_x, dtol_y, dtol_z) );
q.setRPY(0, 0, 0);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, jstate->header.stamp, "dynamixel_link", "laser"));
}
void updateReferenceFramesCallback(const ros::TimerEvent&) {
static tf::TransformBroadcaster br;
tf::Quaternion yarp_to_ros; yarp_to_ros.setRPY(M_PI, -M_PI/2.0, M_PI*2.0);
tf::Transform transform_yarp_to_ros(yarp_to_ros);
auto function_map = std::map<std::string, std::function<bool (yarp::sig::Vector&, yarp::sig::Vector&)> > {
{"robot_head", std::bind(&yarp::dev::IGazeControl::getHeadPose, igaze, std::placeholders::_1, std::placeholders::_2, nullptr)},
{"robot_left_eye", std::bind(&yarp::dev::IGazeControl::getLeftEyePose, igaze, std::placeholders::_1, std::placeholders::_2, nullptr)},
{"robot_right_eye", std::bind(&yarp::dev::IGazeControl::getRightEyePose, igaze, std::placeholders::_1, std::placeholders::_2, nullptr)}
};
for (auto& f : function_map) {
yarp::sig::Vector origin; origin.resize(3);
yarp::sig::Vector orientation; orientation.resize(4);
if(f.second(origin, orientation)) {
tf::Quaternion q(vpt::yarp_to_tf_converter(orientation), orientation[3]);
tf::Transform transform_yarp(q, vpt::yarp_to_tf_converter(origin));
tf::Transform transform = transform_yarp*transform_yarp_to_ros;
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "robot_root", f.first));
} else {
ROS_ERROR_STREAM("Could not retrieve transformation for " << f.first);
}
}
}
// Publish robot odometry tf and topic depending
void PublishOdometry()
{
ros::Time current_time = ros::Time::now();
//first, we'll publish the transform over tf
// TODO change to tf_prefix
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_footprint";
odom_trans.transform.translation.x = robot_pose_px_;
odom_trans.transform.translation.y = robot_pose_py_;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation.x = orientation_x_;
odom_trans.transform.rotation.y = orientation_y_;
odom_trans.transform.rotation.z = orientation_z_;
odom_trans.transform.rotation.w = orientation_w_;
// send the transform over /tf
// activate / deactivate with param
// this tf in needed when not using robot_pose_ekf
if (publish_odom_tf_) odom_broadcaster.sendTransform(odom_trans);
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";
//set the position
// Position
odom.pose.pose.position.x = robot_pose_px_;
odom.pose.pose.position.y = robot_pose_py_;
odom.pose.pose.position.z = 0.0;
// Orientation
odom.pose.pose.orientation.x = orientation_x_;
odom.pose.pose.orientation.y = orientation_y_;
odom.pose.pose.orientation.z = orientation_z_;
odom.pose.pose.orientation.w = orientation_w_;
// Pose covariance
for(int i = 0; i < 6; i++)
odom.pose.covariance[i*6+i] = 0.1; // test 0.001
//set the velocity
odom.child_frame_id = "base_footprint";
// Linear velocities
odom.twist.twist.linear.x = robot_pose_vx_;
odom.twist.twist.linear.y = robot_pose_vy_;
odom.twist.twist.linear.z = 0.0;
// Angular velocities
odom.twist.twist.angular.x = ang_vel_x_;
odom.twist.twist.angular.y = ang_vel_y_;
odom.twist.twist.angular.z = ang_vel_z_;
// Twist covariance
for(int i = 0; i < 6; i++)
odom.twist.covariance[6*i+i] = 0.1; // test 0.001
//publish the message
odom_pub_.publish(odom);
}
/**
* @brief Moves MyPlayer
*
* @param displacement the liner movement of the player, bounded by [-0.1, 1]
* @param turn_angle the turn angle of the player, bounded by [-M_PI/30, M_PI/30]
*/
void move(double displacement, double turn_angle)
{
//Put arguments withing authorized boundaries
double max_d = 1;
displacement = (displacement > max_d ? max_d : displacement);
double min_d = -0.1;
displacement = (displacement < min_d ? min_d : displacement);
double max_t = (M_PI/30);
if (turn_angle > max_t)
turn_angle = max_t;
else if (turn_angle < -max_t)
turn_angle = -max_t;
//Compute the new reference frame
tf::Transform t_mov;
t_mov.setOrigin( tf::Vector3(displacement , 0, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, turn_angle);
t_mov.setRotation(q);
tf::Transform t = getPose();
t = t * t_mov;
//Send the new transform to ROS
br.sendTransform(tf::StampedTransform(t, ros::Time::now(), "/map", name));
}
void BaseFootprint::jsCallback(const sensor_msgs::JointState::ConstPtr & ptr)
{
ros::Time time = ptr->header.stamp;
tf::StampedTransform tf_odom_to_base, tf_odom_to_left_foot, tf_odom_to_right_foot;
ROS_DEBUG("JointState callback function, computing frame %s", m_baseFootPrintID.c_str());
try {
m_listener.lookupTransform(m_odomFrameId, m_lfootFrameId, time, tf_odom_to_left_foot);
m_listener.lookupTransform(m_odomFrameId, m_rfootFrameId, time, tf_odom_to_right_foot);
m_listener.lookupTransform(m_odomFrameId, m_baseFrameId, time, tf_odom_to_base);
} catch (const tf::TransformException& ex){
ROS_ERROR("%s",ex.what());
return ;
}
tf::Vector3 new_origin = (tf_odom_to_right_foot.getOrigin() + tf_odom_to_left_foot.getOrigin())/2.0; // middle of both feet
double height = std::min(tf_odom_to_left_foot.getOrigin().getZ(), tf_odom_to_right_foot.getOrigin().getZ()); // fix to lowest foot
new_origin.setZ(height);
// adjust yaw according to torso orientation, all other angles 0 (= in z-plane)
double roll, pitch, yaw;
tf_odom_to_base.getBasis().getRPY(roll, pitch, yaw);
tf::Transform tf_odom_to_footprint(tf::createQuaternionFromYaw(yaw), new_origin);
tf::Transform tf_base_to_footprint = tf_odom_to_base.inverse() * tf_odom_to_footprint;
// publish transform with parent m_baseFrameId and new child m_baseFootPrintID
// i.e. transform from m_baseFrameId to m_baseFootPrintID
m_brBaseFootPrint.sendTransform(tf::StampedTransform(tf_base_to_footprint, time, m_baseFrameId, m_baseFootPrintID));
ROS_DEBUG("Published Transform %s --> %s", m_baseFrameId.c_str(), m_baseFootPrintID.c_str());
return;
}
void handle_odom(const nav_msgs::Odometry::ConstPtr& msg)
{
tf::Transform transform;
poseMsgToTF(msg->pose.pose, transform);
tf::StampedTransform stamped_transform(transform, msg->header.stamp, msg->header.frame_id, msg->child_frame_id);
tf_br.sendTransform(stamped_transform);
}
void TeleopMaximus::get_value_and_do_computation(void) {
float rotation = 0.0;
if(ser_fd != -1) {
my_maximus_pose.pose.position.x = ((float)TeleopMaximus::read_serial_port('x')) / 10000.0;
my_maximus_pose.pose.position.y = ((float)TeleopMaximus::read_serial_port('y')) / 10000.0;
rotation = TeleopMaximus::read_serial_port('t');
TeleopMaximus::rotate(0, (( (float)rotation) / 10000.0 ), 0, &my_maximus_pose);
// reset laser scan
// my_laser_scan.ranges.std::vector<float>::clear();
// set the new values
//my_laser_scan.ranges.std::vector<float>::push_back(((float)read_serial_port('l')) / 1000.0);
//my_laser_scan.ranges.std::vector<float>::push_back(((float)read_serial_port('m')) / 1000.0);
//my_laser_scan.ranges.std::vector<float>::push_back(((float)read_serial_port('r')) / 1000.0);
}
else {
//TeleopMaximus::rotate(0, ((i)*3.1415/180), 0, &my_maximus_pose);
//rotation = -((i)*3.1415/180) *10000;
//my_maximus_pose.pose.position.y = ((float)i)/40;
heading -= (angular_value / 120000 );
rotation = heading *10000;
TeleopMaximus::rotate(0, -(heading), 0, &my_maximus_pose);
my_maximus_pose.pose.position.x += (linear_value * cos(-heading) / 100000);
my_maximus_pose.pose.position.y += (linear_value * sin(-heading) / 100000);
}
// Actualize Robot's position
my_maximus_pose.header.stamp = ros::Time::now();
// Actualize Robot's path
my_maximus_path.header.stamp = ros::Time::now();
if(my_maximus_path.poses.std::vector<geometry_msgs::PoseStamped>::size() > (my_maximus_path.poses.std::vector<geometry_msgs::PoseStamped>::max_size()-2)) {
my_maximus_path.poses.std::vector<geometry_msgs::PoseStamped>::pop_back();
}
my_maximus_path.poses.std::vector<geometry_msgs::PoseStamped>::push_back(my_maximus_pose);
marker.lifetime = ros::Duration();
// Publish the marker
//marker.header.stamp = ros::Time::now();
//marker_pub.publish(marker);
transform.setOrigin( tf::Vector3(my_maximus_pose.pose.position.x, my_maximus_pose.pose.position.y, 0.0) );
transform.setRotation( tf::Quaternion((( (float)rotation) / 10000.0 ), 0, 0) );
br.sendTransform(tf::StampedTransform(transform, my_maximus_pose.header.stamp, "/map", "/maximus_robot_tf"));
marker.header.stamp = ros::Time::now();
my_laser_scan.header.stamp = ros::Time::now();
i = i + 5;
if( i > 180)
i = -180;
ROS_INFO("X=%f /Y=%f /T=%f /L=%f", my_maximus_pose.pose.position.x, my_maximus_pose.pose.position.y, my_maximus_pose.pose.orientation.z*180/3.1415, linear_value);
}
/* Update formation centroid state */
void SwarmFormationControl::updateCentroid()
{
double x,y,z;
x=y=z=0.0;
std::map<std::string, boost::shared_ptr<Robot> >::iterator it_robot;
for(it_robot = robot_.begin(); it_robot != robot_.end(); ++it_robot)
{
x+=it_robot->second->current_state_world_(0,0);
y+=it_robot->second->current_state_world_(1,0);
z+=it_robot->second->current_state_world_(2,0);
}
x/=robot_.size();
y/=robot_.size();
z/=robot_.size();
std::string centroid_name="swarm_centroid";
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin( tf::Vector3(x, y, z) );
tf::Quaternion q;
q.setRPY(0, 0, 0);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", centroid_name));
geometry_msgs::PoseStamped centroid_msg;
centroid_msg.pose.orientation.x=q.getX();
centroid_msg.pose.orientation.y=q.getY();
centroid_msg.pose.orientation.z=q.getZ();
centroid_msg.pose.orientation.w=q.getW();
centroid_msg.pose.position.x=x;
centroid_msg.pose.position.y=y;
centroid_msg.pose.position.z=z;
swarm_centroid_pub.publish(centroid_msg);
}
//-----------------------------------
void TfRobotSubscriber::updateHook()
{
size.read(datasize);
std::vector<double> vec(3,100),rot(4,100);
entree.read(dataCState);
for(unsigned int i = 1 ; i< dataCState.size() ; i++)
{
dataCState[i].frame_id = "idOfTheRobotSegment" ;
dataCState[i].vector = vec;
dataCState[i].rotation = rot;
}
RTT::FlowStatus fs = entree.read(dataCState);
/*
if(fs == RTT::NewData)
{
std::cout << "Got New Data " << std::endl;
}
else
{
std::cout << "No new Data " << std::endl;
}
*/
for(unsigned int i=1 ; i < dataCState.size(); i++)
{
robot_transform.setRotation( tf::Quaternion(dataCState[i].rotation[0], dataCState[i].rotation[1], dataCState[i].rotation[2], dataCState[i].rotation[3] ) );
robot_transform.setOrigin( tf::Vector3(dataCState[i].vector[0],dataCState[i].vector[1],dataCState[i].vector[2]) );
static tf::TransformBroadcaster br;
br.sendTransform(tf::StampedTransform(robot_transform, ros::Time::now(), "world",dataCState[i].frame_id ));
}
}
void callback(const std_msgs::Float64MultiArray::ConstPtr& msg)
{
static tf::TransformBroadcaster br;
tf::Transform transform;
// tf::Quaternion q;
// float pan_angleRAD = msg->data[0] * pi/180;
// float tilt_angleRAD = msg->data[1] * pi/180;
float th_p = msg->data[0] * pi/180;
float th_t = msg->data[1] * pi/180;
float px1 = 0.095;
float py1 = -0.04;
float pz1 = 0.33;
float a = 0.02;
float pxt = cos(th_t)*(px1*cos(th_p)-py1*sin(th_p)) + sin(th_t)*pz1 + a*cos(th_t);
float pyt = px1*sin(th_p) + py1*cos(th_p);
float pzt = -sin(th_t)*(px1*cos(th_p)-py1*sin(th_p)) + cos(th_t)*pz1 - a*sin(th_t);
tf::Matrix3x3 tf3d;
tf3d.setValue(cos(th_t)*cos(th_p), -cos(th_t)*sin(th_p), sin(th_t),
sin(th_p), cos(th_p), 0.0,
-sin(th_t)*cos(th_p), sin(th_t)*sin(th_p), cos(th_t));
tf::Quaternion tfqt;
tf3d.getRotation(tfqt);
transform.setRotation(tfqt);
transform.setOrigin(tf::Vector3(pxt, pyt, pzt));
//q.setRPY(0, 0, pan_angleRAD);
// transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
"camera_depth_optical_frame", "webcam"));
}
void PlatformCtrlNode::receiveOdo(const sensor_msgs::JointState& js)
{
mutex.lock();
//odometry msgs
nav_msgs::Odometry odom;
odom.header.stamp = ros::Time::now();
odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";
kin->execForwKin(js, odom, p);
topicPub_Odometry.publish(odom);
//odometry transform:
if(sendTransform)
{
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = odom.header.stamp;
odom_trans.header.frame_id = odom.header.frame_id;
odom_trans.child_frame_id = odom.child_frame_id;
odom_trans.transform.translation.x = odom.pose.pose.position.x;
odom_trans.transform.translation.y = odom.pose.pose.position.y;
odom_trans.transform.translation.z = odom.pose.pose.position.z;
odom_trans.transform.rotation = odom.pose.pose.orientation;
odom_broadcaster.sendTransform(odom_trans);
}
mutex.unlock();
}
void velPublishtoEKF(void)
{
geometry_msgs::TwistWithCovarianceStamped v;
v.header.stamp = ros::Time::now();
v.header.frame_id = "vel";
//# create a new velocity
v.twist.twist.linear.x = 0.0;
v.twist.twist.linear.y = 0.0;
v.twist.twist.linear.z = 0.0;
//# Only the number in the covariance matrix diagonal
//# are used for the updates!
v.twist.covariance[0] = 0.01;
v.twist.covariance[7] = 0.01;
v.twist.covariance[14] = 0.01;
velpublish.publish(v);
///publish TF
static tf::TransformBroadcaster publishtf;
tf::Transform transform;
transform.setOrigin( tf::Vector3( 0, 0, 0.0) );
tf::Quaternion q;
q.setRPY(/**msg->theta**/0, 0, 0);
transform.setRotation(q);
publishtf.sendTransform(tf::StampedTransform(transform, v.header.stamp,"robot", v.header.frame_id)); ///parent sikik "robot", njut child (kuwalik karo sing piton)
}
void QRposeCallback(const geometry_msgs::PoseStamped& msg){
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z) );
transform.setRotation( tf::Quaternion(msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "qr", "cam"));
}
void GpsTransformPublisher::HandleGps(const gps_common::GPSFixPtr& gps_fix)
{
tf::Transform transform;
// Get the orientation from the GPS track.
// NOTE: This will be unreliable when the vehicle is stopped or moving at low
// speed.
double yaw = (90.0 - gps_fix->track) * swri_math_util::_deg_2_rad;
yaw = swri_math_util::WrapRadians(yaw, swri_math_util::_pi);
tf::Quaternion orientation;
orientation.setRPY(0, 0, yaw);
transform.setRotation(orientation);
// Get the position by converting lat/lon to LocalXY.
swri_transform_util::Transform to_local_xy;
if (tf_manager_.GetTransform(global_frame_id_, swri_transform_util::_wgs84_frame, ros::Time(0), to_local_xy))
{
tf::Vector3 position(gps_fix->longitude, gps_fix->latitude, gps_fix->altitude);
position = to_local_xy * position;
transform.setOrigin(position);
tf_.sendTransform(tf::StampedTransform(
transform,
gps_fix->header.stamp,
global_frame_id_,
veh_frame_id_));
}
}
bool callService::callTheService()
{
if(client_.call(srv_)){
double x,y,z,qx,qy,qz,qw;
x = srv_.response.final_pose.position.x;
y = srv_.response.final_pose.position.y;
z = srv_.response.final_pose.position.z;
qx = srv_.response.final_pose.orientation.x;
qy = srv_.response.final_pose.orientation.y;
qz = srv_.response.final_pose.orientation.z;
qw = srv_.response.final_pose.orientation.w;
ROS_INFO("Pose: tx= %5.4lf %5.4lf %5.4lf quat= %5.3lf %5.3lf %5.3lf %5.3lf, cost= %5.3lf",
srv_.response.final_pose.position.x,
srv_.response.final_pose.position.y,
srv_.response.final_pose.position.z,
srv_.response.final_pose.orientation.x,
srv_.response.final_pose.orientation.y,
srv_.response.final_pose.orientation.z,
srv_.response.final_pose.orientation.w,
srv_.response.final_cost_per_observation);
tf::Transform camera_to_target;
tf::Quaternion quat(qx,qy,qz,qw);
camera_to_target.setOrigin(tf::Vector3(x,y,z));
camera_to_target.setRotation(quat);
tf::StampedTransform stf(camera_to_target, ros::Time::now(), optical_frame_.c_str(), "target_frame");
tf_broadcaster_.sendTransform(stf);
return(true);
}
ROS_ERROR("Target Location Failure");
return(false);
}
void tfRegistration (const cv::Mat &camExtMat, const ros::Time& timeStamp)
{
tf::Matrix3x3 rotation_mat;
double roll = 0, pitch = 0, yaw = 0;
tf::Quaternion quaternion;
tf::Transform transform;
static tf::TransformBroadcaster broadcaster;
rotation_mat.setValue(camExtMat.at<double>(0, 0), camExtMat.at<double>(0, 1), camExtMat.at<double>(0, 2),
camExtMat.at<double>(1, 0), camExtMat.at<double>(1, 1), camExtMat.at<double>(1, 2),
camExtMat.at<double>(2, 0), camExtMat.at<double>(2, 1), camExtMat.at<double>(2, 2));
rotation_mat.getRPY(roll, pitch, yaw, 1);
quaternion.setRPY(roll, pitch, yaw);
transform.setOrigin(tf::Vector3(camExtMat.at<double>(0, 3),
camExtMat.at<double>(1, 3),
camExtMat.at<double>(2, 3)));
transform.setRotation(quaternion);
broadcaster.sendTransform(tf::StampedTransform(transform, timeStamp, "velodyne", camera_id_str));
//broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "velodyne", "camera"));
}
void poseCallback(const turtlesim::PoseConstPtr& msg){
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
transform.setRotation( tf::Quaternion(0, 0, msg->theta) );
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
// now that we are calibrated, we are free to send the transforms:
void send_frames(void)
{
ROS_DEBUG("Sending frames");
tf::Transform transform;
transform.setOrigin(tf::Vector3(cal_pos(0),
cal_pos(1),
cal_pos(2)));
transform.setRotation(tf::Quaternion(0,0,0,1));
br.sendTransform(tf::StampedTransform(transform, tstamp,
"/oriented_optimization_frame",
"/optimization_frame"));
// Publish /map frame based on robot calibration
transform.setOrigin(tf::Vector3(cal_pos(0),
0,
cal_pos(2)));
transform.setRotation(tf::Quaternion(.707107,0.0,0.0,-0.707107));
br.sendTransform(tf::StampedTransform(transform, tstamp,
"/oriented_optimization_frame",
"/map"));
// publish one more frame that is the frame the robot
// calculates its odometry in.
transform.setOrigin(tf::Vector3(0,0,0));
transform.setRotation(tf::Quaternion(1,0,0,0));
br.sendTransform(tf::StampedTransform(transform, tstamp,
"/map",
"/robot_odom_pov"));
ROS_DEBUG("frames sent");
return;
}