예제 #1
0
파일: main_pub.cpp 프로젝트: jiapei100/mrpt
int main()
{
	try
	{
		printf ("Starting publisher...\n");

		void *context = zmq_ctx_new ();
		void *pub_sock = zmq_socket (context, ZMQ_PUB);
		int rc = zmq_bind (pub_sock, "tcp://*:5555");
		assert (rc == 0);

		while (1)
		{
			mrpt::poses::CPose3D  my_pose(0.5f,0.5f,1.5f ,DEG2RAD(-90.0f),DEG2RAD(0),DEG2RAD(-90.0f)  );
			printf("Publishing pose...\n");
			mrpt::comms::mrpt_send_to_zmq(pub_sock, my_pose);
			std::this_thread::sleep_for(100ms);

			mrpt::img::CImage my_img(800,600, CH_RGB);
			printf("Publishing img...\n");
			mrpt::comms::mrpt_send_to_zmq(pub_sock, my_img, 0 /* max_packet_len: 0=no max size */);
			std::this_thread::sleep_for(100ms);
		}


		zmq_close (pub_sock);
		zmq_ctx_destroy (context);
		return 0;
	} catch (std::exception &e)
	{
		std::cerr << "**Exception**: " << e.what() << std::endl;
		return -1;
	}
}
예제 #2
0
TEST(CPose3DInterpolator, interp)
{
	using namespace mrpt::poses;
	using mrpt::math::TPose3D;
	using mrpt::math::CMatrixDouble44;
	using mrpt::DEG2RAD;

	const mrpt::system::TTimeStamp t0 = mrpt::system::now();
	const mrpt::system::TTimeStamp dt = mrpt::system::secondsToTimestamp(0.10);

	CPose3DInterpolator pose_path;

	pose_path.insert(
		t0, TPose3D(1., 2., 3., DEG2RAD(30.0), DEG2RAD(.0), DEG2RAD(.0)));
	pose_path.insert(
		t0 + 2 * dt, TPose3D(
						 1. + 3., 2. + 4., 3. + 5., DEG2RAD(30.0 + 20.0),
						 DEG2RAD(.0), DEG2RAD(.0)));

	TPose3D interp;
	bool valid;
	pose_path.interpolate(t0 + dt, interp, valid);

	EXPECT_TRUE(valid);
	const TPose3D interp_good(
		1. + 1.5, 2. + 2.0, 3. + 2.5, DEG2RAD(30.0 + 10.0), DEG2RAD(.0),
		DEG2RAD(.0));
	EXPECT_NEAR(
		.0, (CPose3D(interp_good).getHomogeneousMatrixVal<CMatrixDouble44>() -
			 CPose3D(interp).getHomogeneousMatrixVal<CMatrixDouble44>())
				.array()
				.abs()
				.sum(),
		1e-4);
}
예제 #3
0
void TPose3D::fromString(const std::string& s)
{
	CMatrixDouble m;
	if (!m.fromMatlabStringFormat(s))
		THROW_EXCEPTION("Malformed expression in ::fromString");
	ASSERTMSG_(
		m.rows() == 1 && m.cols() == 6, "Wrong size of vector in ::fromString");
	x = m.get_unsafe(0, 0);
	y = m.get_unsafe(0, 1);
	z = m.get_unsafe(0, 2);
	yaw = DEG2RAD(m.get_unsafe(0, 3));
	pitch = DEG2RAD(m.get_unsafe(0, 4));
	roll = DEG2RAD(m.get_unsafe(0, 5));
}
예제 #4
0
void TTwist3D::fromString(const std::string& s)
{
	CMatrixDouble m;
	if (!m.fromMatlabStringFormat(s))
		THROW_EXCEPTION("Malformed expression in ::fromString");
	ASSERTMSG_(
		m.rows() == 1 && m.cols() == 6, "Wrong size of vector in ::fromString");
	for (int i = 0; i < 3; i++) (*this)[i] = m.get_unsafe(0, i);
	for (int i = 0; i < 3; i++)
		(*this)[3 + i] = DEG2RAD(m.get_unsafe(0, 3 + i));
}
예제 #5
0
void TTwist2D::fromString(const std::string& s)
{
	CMatrixDouble m;
	if (!m.fromMatlabStringFormat(s))
		THROW_EXCEPTION("Malformed expression in ::fromString");
	ASSERTMSG_(
		m.rows() == 1 && m.cols() == 3, "Wrong size of vector in ::fromString");
	vx = m.get_unsafe(0, 0);
	vy = m.get_unsafe(0, 1);
	omega = DEG2RAD(m.get_unsafe(0, 2));
}
TEST(CPose3DInterpolator, interp)
{
	using namespace mrpt::poses;
	using mrpt::DEG2RAD;
	using mrpt::math::CMatrixDouble44;
	using mrpt::math::TPose3D;

	auto t0 = mrpt::Clock::now();
	mrpt::Clock::duration dt(std::chrono::milliseconds(100));

	CPose3DInterpolator pose_path;

	pose_path.insert(
		t0, TPose3D(1., 2., 3., DEG2RAD(30.0), DEG2RAD(.0), DEG2RAD(.0)));
	pose_path.insert(
		t0 + 2 * dt, TPose3D(
						 1. + 3., 2. + 4., 3. + 5., DEG2RAD(30.0 + 20.0),
						 DEG2RAD(.0), DEG2RAD(.0)));

	TPose3D interp;
	bool valid;
	pose_path.interpolate(t0 + dt, interp, valid);

	EXPECT_TRUE(valid);
	const TPose3D interp_good(
		1. + 1.5, 2. + 2.0, 3. + 2.5, DEG2RAD(30.0 + 10.0), DEG2RAD(.0),
		DEG2RAD(.0));
	EXPECT_NEAR(
		.0,
		(CPose3D(interp_good).getHomogeneousMatrixVal<CMatrixDouble44>() -
		 CPose3D(interp).getHomogeneousMatrixVal<CMatrixDouble44>())
			.array()
			.abs()
			.sum(),
		2e-4);
}