Connectvity with other controllers

[wogirzl]

Good morning!
I have successfully connected two Vive controller. May I ask if it is also possible also to connect a Vive tracker additionally or instead of one controller?
Thanks in advance!

[ishiguroJSK]

vive_ros/src/vive_node.cpp

Lines 648 to 652 in 0b55e8f

	// It's a tracker
	if (dev_type == 3)
	{
	tf_broadcaster_.sendTransform(tf::StampedTransform(tf, ros::Time::now(), "world_vive", "tracker_"+cur_sn));
	}

I don't know which version of SteamVR Tracking and which ROS distro you are using,
but I confirmed trackers work well with SteamVR Tracking 1.0 and ROS indigo~kinetic.

[wogirzl]

Have you tried this on your own if it works?
If fund also this lines of code:

vive_ros/src/vive_node.cpp

Lines 531 to 534 in 0b55e8f

	while (dev_type != 2)
	{
	dev_type = vr_.GetDeviceMatrix(index++, tf_matrix);
	}

This implies for me that there could be a limitation for controller only.
I am using ROS melodic and I am not sure how to find out which SteamVR Traking system it is.
But in general everything is working with both controllers and HMD 👍

[ishiguroJSK]

Ofcourse several features are limited.
That feature is resetting the world origin position that is triggered by the controller's button,
but if you really want it, you can manually set with rosparam

vive_ros/src/vive_node.cpp

Lines 561 to 564 in 0b55e8f

	nh_.setParam("/vive/world_offset", world_offset_);
	nh_.setParam("/vive/world_yaw", world_yaw_);
	ROS_INFO(" [VIVE] New world offset: [%2.3f , %2.3f, %2.3f] %2.3f", world_offset_[0], world_offset_[1], world_offset_[2], world_yaw_);

The latest VIVE products are SteamVR Tracking 2.0
https://www.vive.com/us/accessory/base-station2/
If not, you are using ver 1.0.

[ishiguroJSK]

Sorry, the "setOriginCB" seems not to be called by button trigger but by manually rosservice call.

[wogirzl]

Okay, thank you. It looks like that the issue is with the topic /vive/twist1 and /vive/twist2. So ROS Melodic is running together with SteamVR Tracking 2.0. Everything is working quite well. /vive/twist0 is publishing the linear and angular velocities of the HMD but twist1 and twist2 are sending just zeros. On the contrary, the topics of the controller sending the button state and the finger position on the pad, are working. It looks like that the /tf topic is giving all velocities also those of the controllers. But I am not sure why twist1 and twist2 are not working.

[ishiguroJSK]

In the current code, tf and button functions are implemented in this loop, and this loop scans all connected devices.

vive_ros/src/vive_node.cpp

Lines 589 to 657 in 0b55e8f

	for (int i=0; i<vr::k_unMaxTrackedDeviceCount; i++)
	{
	int dev_type = vr_.GetDeviceMatrix(i, tf_matrix);
	// No device
	if (dev_type == 0) continue;
	tf::Transform tf;
	tf.setOrigin(tf::Vector3(tf_matrix[0][3], tf_matrix[1][3], tf_matrix[2][3]));
	tf::Quaternion quat;
	tf::Matrix3x3 rot_matrix(tf_matrix[0][0], tf_matrix[0][1], tf_matrix[0][2],
	tf_matrix[1][0], tf_matrix[1][1], tf_matrix[1][2],
	tf_matrix[2][0], tf_matrix[2][1], tf_matrix[2][2]);
	rot_matrix.getRotation(quat);
	tf.setRotation(quat);
	//get device serial number
	std::string cur_sn = GetTrackedDeviceString( vr_.pHMD_, i, vr::Prop_SerialNumber_String );
	std::replace(cur_sn.begin(), cur_sn.end(), '-', '_');
	// It's a HMD
	if (dev_type == 1)
	{
	tf_broadcaster_.sendTransform(tf::StampedTransform(tf, ros::Time::now(), "world_vive", "hmd"));
	}
	// It's a controller
	if (dev_type == 2)
	{
	tf_broadcaster_.sendTransform(tf::StampedTransform(tf, ros::Time::now(), "world_vive", "controller_"+cur_sn));
	vr::VRControllerState_t state;
	vr_.HandleInput(i, state);
	sensor_msgs::Joy joy;
	joy.header.stamp = ros::Time::now();
	joy.header.frame_id = "controller_"+cur_sn;
	joy.buttons.assign(BUTTON_NUM, 0);
	joy.axes.assign(AXES_NUM, 0.0); // x-axis, y-axis
	if((1LL << vr::k_EButton_ApplicationMenu) & state.ulButtonPressed)
	joy.buttons[0] = 1;
	if((1LL << vr::k_EButton_SteamVR_Trigger) & state.ulButtonPressed)
	joy.buttons[1] = 1;
	if((1LL << vr::k_EButton_SteamVR_Touchpad) & state.ulButtonPressed)
	joy.buttons[2] = 1;
	if((1LL << vr::k_EButton_Grip) & state.ulButtonPressed)
	joy.buttons[3] = 1;
	// TrackPad's axis
	joy.axes[0] = state.rAxis[0].x;
	joy.axes[1] = state.rAxis[0].y;
	// Trigger's axis
	joy.axes[2] = state.rAxis[1].x;
	// #include <bitset> // bit debug
	// std::cout << static_cast<std::bitset<64> >(state.ulButtonPressed) << std::endl;
	// std::cout << static_cast<std::bitset<64> >(state.ulButtonTouched) << std::endl;
	if(button_states_pubs_map.count(cur_sn) == 0){
	button_states_pubs_map[cur_sn] = nh_.advertise<sensor_msgs::Joy>("/vive/controller_"+cur_sn+"/joy", 10);
	}
	button_states_pubs_map[cur_sn].publish(joy);
	}
	// It's a tracker
	if (dev_type == 3)
	{
	tf_broadcaster_.sendTransform(tf::StampedTransform(tf, ros::Time::now(), "world_vive", "tracker_"+cur_sn));
	}
	// It's a lighthouse
	if (dev_type == 4)
	{
	tf_broadcaster_.sendTransform(tf::StampedTransform(tf, ros::Time::now(), "world_vive", "lighthouse_"+cur_sn));
	}

But the twist function is currently implemented in the other way, and it only accesses device id 0~2.

vive_ros/src/vive_node.cpp

Lines 670 to 731 in 0b55e8f

	// Publish twist messages for controller1 and controller2
	double lin_vel[3], ang_vel[3];
	if (vr_.GetDeviceVel(0, lin_vel, ang_vel))
	{
	geometry_msgs::Twist twist_msg;
	twist_msg.linear.x = lin_vel[0];
	twist_msg.linear.y = lin_vel[1];
	twist_msg.linear.z = lin_vel[2];
	twist_msg.angular.x = ang_vel[0];
	twist_msg.angular.y = ang_vel[1];
	twist_msg.angular.z = ang_vel[2];
	geometry_msgs::TwistStamped twist_msg_stamped;
	twist_msg_stamped.header.stamp = ros::Time::now();
	twist_msg_stamped.header.frame_id = "world_vive";
	twist_msg_stamped.twist = twist_msg;
	twist0_pub_.publish(twist_msg_stamped);
	// std::cout<<"HMD:";
	// std::cout<<twist_msg_stamped;
	}
	if (vr_.GetDeviceVel(1, lin_vel, ang_vel))
	{
	geometry_msgs::Twist twist_msg;
	twist_msg.linear.x = lin_vel[0];
	twist_msg.linear.y = lin_vel[1];
	twist_msg.linear.z = lin_vel[2];
	twist_msg.angular.x = ang_vel[0];
	twist_msg.angular.y = ang_vel[1];
	twist_msg.angular.z = ang_vel[2];
	geometry_msgs::TwistStamped twist_msg_stamped;
	twist_msg_stamped.header.stamp = ros::Time::now();
	twist_msg_stamped.header.frame_id = "world_vive";
	twist_msg_stamped.twist = twist_msg;
	twist1_pub_.publish(twist_msg_stamped);
	// std::cout<<"Controller 1:";
	// std::cout<<twist_msg_stamped;
	}
	if (vr_.GetDeviceVel(2, lin_vel, ang_vel))
	{
	geometry_msgs::Twist twist_msg;
	twist_msg.linear.x = lin_vel[0];
	twist_msg.linear.y = lin_vel[1];
	twist_msg.linear.z = lin_vel[2];
	twist_msg.angular.x = ang_vel[0];
	twist_msg.angular.y = ang_vel[1];
	twist_msg.angular.z = ang_vel[2];
	geometry_msgs::TwistStamped twist_msg_stamped;
	twist_msg_stamped.header.stamp = ros::Time::now();
	twist_msg_stamped.header.frame_id = "world_vive";
	twist_msg_stamped.twist = twist_msg;
	twist2_pub_.publish(twist_msg_stamped);
	// std::cout<<"Controller 2:";
	// std::cout<<twist_msg_stamped;
	}

I think some code modifications are needed to enable all connected devices to publish twist.