void PlayerCharacter::fire() {
auto gunX = copyPosition().getX() + (getSize()->getWidth() / 2) ;
auto gunY = copyPosition().getY() + (getSize()->getWidth() / 2) ;
Vect<float> gunPos { gunX, gunY} ;
weapon.fire(gunPos, * getOrientation()) ;
}
void MIAtom::copyShallow(const MIAtom &atom)
{
setName(atom.name_);
type_ = atom.type_;
BValue_ = atom.BValue_;
occ_ = atom.occ_;
color_ = atom.color_;
copyPosition(atom);
dx_ = atom.dx_;
dy_ = atom.dy_;
dz_ = atom.dz_;
weight_ = atom.weight_;
radius_type_ = atom.radius_type_;
symmop_ = atom.symmop_;
altloc_ = atom.altloc_;
atomnumber_ = atom.atomnumber_;
atomicnumber_ = atom.atomicnumber_;
isaromatic_ = atom.isaromatic_;
copyChemicalData(atom);
ring_system_ = atom.ring_system_;
search_flag_ = atom.search_flag_;
if (atom.hasAnisotropicity())
{
newAnisotropicity();
memcpy(U_, atom.U_, 6*sizeof(float));
}
}
void publishRecentlyVistedSteps(ros::Publisher& pub, const std::vector<msgs::Step>& recently_visited_steps, const std_msgs::Header& header)
{
// populate point cloud
pcl::PointCloud<pcl::PointXYZ> point_cloud;
point_cloud.resize(recently_visited_steps.size());
for (unsigned long i = 0; i < recently_visited_steps.size(); i++)
{
pcl::PointXYZ& point = point_cloud.points[i];
copyPosition(recently_visited_steps[i].foot.pose.position, point);
}
// publish as point cloud msg
sensor_msgs::PointCloud2 point_cloud_msgs;
pcl::toROSMsg(point_cloud, point_cloud_msgs);
point_cloud_msgs.header = header;
pub.publish(point_cloud_msgs);
}
void publishVistedSteps(ros::Publisher& pub, const std::vector<msgs::Step>& visited_steps, const std_msgs::Header& header)
{
// populate point cloud
pcl::PointCloud<pcl::PointXYZI> point_cloud;
point_cloud.resize(visited_steps.size());
ros::Time current_time = ros::Time::now();
unsigned long i = 0;
for (std::vector<msgs::Step>::const_iterator itr = visited_steps.begin(); itr != visited_steps.end(); itr++, i++)
{
pcl::PointXYZI& point = point_cloud.points[i];
copyPosition(itr->foot.pose.position, point);
point.intensity = std::min(5.0, (current_time - itr->header.stamp).toSec());
}
// publish as point cloud msg
sensor_msgs::PointCloud2 point_cloud_msgs;
pcl::toROSMsg(point_cloud, point_cloud_msgs);
point_cloud_msgs.header = header;
pub.publish(point_cloud_msgs);
}
/* Sets pos as the the current position of the state s */
void setStatePosition(State *s, Position pos) {
copyPosition(&s->pos, pos);
}
/* Returns the current position of the state s in pos */
void getStatePosition(State s, Position *pos) {
copyPosition(pos, s.pos);
}
QJSValue THREEMatrix4::extractPosition(QJSValue threeMatrix4) {
qDebug().nospace() << "THREEAccel.Matrix4: .extractPosition() has been renamed to .copyPosition().";
return copyPosition(threeMatrix4);
}
// This gets called by the world update start event.
void GazeboOdometryPlugin::OnUpdate(const common::UpdateInfo& _info) {
// C denotes child frame, P parent frame, and W world frame.
// Further C_pose_W_P denotes pose of P wrt. W expressed in C.
math::Pose W_pose_W_C = link_->GetWorldCoGPose();
math::Vector3 C_linear_velocity_W_C = link_->GetRelativeLinearVel();
math::Vector3 C_angular_velocity_W_C = link_->GetRelativeAngularVel();
math::Vector3 gazebo_linear_velocity = C_linear_velocity_W_C;
math::Vector3 gazebo_angular_velocity = C_angular_velocity_W_C;
math::Pose gazebo_pose = W_pose_W_C;
if (parent_frame_id_ != "world") {
math::Pose W_pose_W_P = parent_link_->GetWorldPose();
math::Vector3 P_linear_velocity_W_P = parent_link_->GetRelativeLinearVel();
math::Vector3 P_angular_velocity_W_P = parent_link_->GetRelativeAngularVel();
math::Pose C_pose_P_C_ = W_pose_W_C - W_pose_W_P;
math::Vector3 C_linear_velocity_P_C;
// \prescript{}{C}{\dot{r}}_{PC} = -R_{CP}
// \cdot \prescript{}{P}{\omega}_{WP} \cross \prescript{}{P}{r}_{PC}
// + \prescript{}{C}{v}_{WC}
// - R_{CP} \cdot \prescript{}{P}{v}_{WP}
C_linear_velocity_P_C = - C_pose_P_C_.rot.GetInverse()
* P_angular_velocity_W_P.Cross(C_pose_P_C_.pos)
+ C_linear_velocity_W_C
- C_pose_P_C_.rot.GetInverse() * P_linear_velocity_W_P;
// \prescript{}{C}{\omega}_{PC} = \prescript{}{C}{\omega}_{WC}
// - R_{CP} \cdot \prescript{}{P}{\omega}_{WP}
gazebo_angular_velocity = C_angular_velocity_W_C
- C_pose_P_C_.rot.GetInverse() * P_angular_velocity_W_P;
gazebo_linear_velocity = C_linear_velocity_P_C;
gazebo_pose = C_pose_P_C_;
}
bool publish_odometry = true;
// First, determine whether we should publish a odometry.
if (covariance_image_.data != NULL) {
// We have an image.
// Image is always centered around the origin:
int width = covariance_image_.cols;
int height = covariance_image_.rows;
int x = static_cast<int>(std::floor(gazebo_pose.pos.x / covariance_image_scale_)) + width / 2;
int y = static_cast<int>(std::floor(gazebo_pose.pos.y / covariance_image_scale_)) + height / 2;
if (x >= 0 && x < width && y >= 0 && y < height) {
uint8_t pixel_value = covariance_image_.at<uint8_t>(y, x);
if (pixel_value == 0) {
publish_odometry = false;
// TODO: covariance scaling, according to the intensity values could be implemented here.
}
}
}
if (gazebo_sequence_ % measurement_divisor_ == 0) {
nav_msgs::Odometry odometry;
odometry.header.frame_id = parent_frame_id_;
odometry.header.seq = odometry_sequence_++;
odometry.header.stamp.sec = (world_->GetSimTime()).sec + ros::Duration(unknown_delay_).sec;
odometry.header.stamp.nsec = (world_->GetSimTime()).nsec + ros::Duration(unknown_delay_).nsec;
odometry.child_frame_id = namespace_;
copyPosition(gazebo_pose.pos, &odometry.pose.pose.position);
odometry.pose.pose.orientation.w = gazebo_pose.rot.w;
odometry.pose.pose.orientation.x = gazebo_pose.rot.x;
odometry.pose.pose.orientation.y = gazebo_pose.rot.y;
odometry.pose.pose.orientation.z = gazebo_pose.rot.z;
odometry.twist.twist.linear.x = gazebo_linear_velocity.x;
odometry.twist.twist.linear.y = gazebo_linear_velocity.y;
odometry.twist.twist.linear.z = gazebo_linear_velocity.z;
odometry.twist.twist.angular.x = gazebo_angular_velocity.x;
odometry.twist.twist.angular.y = gazebo_angular_velocity.y;
odometry.twist.twist.angular.z = gazebo_angular_velocity.z;
if (publish_odometry)
odometry_queue_.push_back(std::make_pair(gazebo_sequence_ + measurement_delay_, odometry));
}
// Is it time to publish the front element?
if (gazebo_sequence_ == odometry_queue_.front().first) {
nav_msgs::OdometryPtr odometry(new nav_msgs::Odometry);
odometry->header = odometry_queue_.front().second.header;
odometry->child_frame_id = odometry_queue_.front().second.child_frame_id;
odometry->pose.pose = odometry_queue_.front().second.pose.pose;
odometry->twist.twist.linear = odometry_queue_.front().second.twist.twist.linear;
odometry->twist.twist.angular = odometry_queue_.front().second.twist.twist.angular;
odometry_queue_.pop_front();
// Calculate position distortions.
Eigen::Vector3d pos_n;
pos_n << position_n_[0](random_generator_) + position_u_[0](random_generator_),
position_n_[1](random_generator_) + position_u_[1](random_generator_),
position_n_[2](random_generator_) + position_u_[2](random_generator_);
geometry_msgs::Point& p = odometry->pose.pose.position;
p.x += pos_n[0];
p.y += pos_n[1];
p.z += pos_n[2];
// Calculate attitude distortions.
Eigen::Vector3d theta;
theta << attitude_n_[0](random_generator_) + attitude_u_[0](random_generator_),
attitude_n_[1](random_generator_) + attitude_u_[1](random_generator_),
attitude_n_[2](random_generator_) + attitude_u_[2](random_generator_);
Eigen::Quaterniond q_n = QuaternionFromSmallAngle(theta);
q_n.normalize();
geometry_msgs::Quaternion& q_W_L = odometry->pose.pose.orientation;
Eigen::Quaterniond _q_W_L(q_W_L.w, q_W_L.x, q_W_L.y, q_W_L.z);
_q_W_L = _q_W_L * q_n;
q_W_L.w = _q_W_L.w();
q_W_L.x = _q_W_L.x();
q_W_L.y = _q_W_L.y();
q_W_L.z = _q_W_L.z();
// Calculate linear velocity distortions.
Eigen::Vector3d linear_velocity_n;
linear_velocity_n << linear_velocity_n_[0](random_generator_) + linear_velocity_u_[0](random_generator_),
linear_velocity_n_[1](random_generator_) + linear_velocity_u_[1](random_generator_),
linear_velocity_n_[2](random_generator_) + linear_velocity_u_[2](random_generator_);
geometry_msgs::Vector3& linear_velocity = odometry->twist.twist.linear;
linear_velocity.x += linear_velocity_n[0];
linear_velocity.y += linear_velocity_n[1];
linear_velocity.z += linear_velocity_n[2];
// Calculate angular veocity distortions.
Eigen::Vector3d angular_velocity_n;
angular_velocity_n << angular_velocity_n_[0](random_generator_) + angular_velocity_u_[0](random_generator_),
angular_velocity_n_[1](random_generator_) + angular_velocity_u_[1](random_generator_),
angular_velocity_n_[2](random_generator_) + angular_velocity_u_[2](random_generator_);
geometry_msgs::Vector3& angular_velocity = odometry->twist.twist.angular;
angular_velocity.x += angular_velocity_n[0];
angular_velocity.y += angular_velocity_n[1];
angular_velocity.z += angular_velocity_n[2];
odometry->pose.covariance = pose_covariance_matrix_;
odometry->twist.covariance = twist_covariance_matrix_;
// Publish all the topics, for which the topic name is specified.
if (pose_pub_.getNumSubscribers() > 0) {
geometry_msgs::PoseStampedPtr pose(new geometry_msgs::PoseStamped);
pose->header = odometry->header;
pose->pose = odometry->pose.pose;
pose_pub_.publish(pose);
}
if (pose_with_covariance_pub_.getNumSubscribers() > 0) {
geometry_msgs::PoseWithCovarianceStampedPtr pose_with_covariance(
new geometry_msgs::PoseWithCovarianceStamped);
pose_with_covariance->header = odometry->header;
pose_with_covariance->pose.pose = odometry->pose.pose;
pose_with_covariance->pose.covariance = odometry->pose.covariance;
pose_with_covariance_pub_.publish(pose_with_covariance);
}
if (position_pub_.getNumSubscribers() > 0) {
geometry_msgs::PointStampedPtr position(new geometry_msgs::PointStamped);
position->header = odometry->header;
position->point = p;
position_pub_.publish(position);
}
if (transform_pub_.getNumSubscribers() > 0) {
geometry_msgs::TransformStampedPtr transform(new geometry_msgs::TransformStamped);
transform->header = odometry->header;
geometry_msgs::Vector3 translation;
translation.x = p.x;
translation.y = p.y;
translation.z = p.z;
transform->transform.translation = translation;
transform->transform.rotation = q_W_L;
transform_pub_.publish(transform);
}
if (odometry_pub_.getNumSubscribers() > 0) {
odometry_pub_.publish(odometry);
}
tf::Quaternion tf_q_W_L(q_W_L.x, q_W_L.y, q_W_L.z, q_W_L.w);
tf::Vector3 tf_p(p.x, p.y, p.z);
tf_ = tf::Transform(tf_q_W_L, tf_p);
transform_broadcaster_.sendTransform(tf::StampedTransform(tf_, odometry->header.stamp, parent_frame_id_, namespace_));
// std::cout << "published odometry with timestamp " << odometry->header.stamp << "at time t" << world_->GetSimTime().Double()
// << "delay should be " << measurement_delay_ << "sim cycles" << std::endl;
}
++gazebo_sequence_;
}
