Ejemplos de Kinematics::execForwKin en C++ (Cpp)

Ejemplo n.º 1

Archivo: herbi_platformctrl_node.cpp Proyecto: TimoHackel/herbi_robot

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();
}