Hi Jon,

On 08/05/2015 10:09 AM, Jon Verbeke wrote:
> Hi,
>
> I have iTaSC running perfectly in Groovy and am porting the last bit of code to ROS Indigo.
> I am using a .ops script to connect several ports and I noticed that the syntax changed from connectTwoPorts() to connect().
> So I adapted my script and have now:
>
> connect("robot.Pose_up_infrared_link_base","ceiling.Pose",ConnPolicy())
> connect("robot.Pose_right_ultra_link_base","right_wall.Pose",ConnPolicy())
> connect("robot.Pose_left_ultra_link_base","left_wall.Pose",ConnPolicy())
> connect("marker.camera_health","robot_itasc_indoor_supervisor.camera_health",ConnPolicy())
>
> The port connections work well except one:
>
> connect("robot.Pose_camera_link_base","marker.Pose",ConnPolicy())
>
> So first question, just to be sure: the command to connect two ports is correct, right?
>
> Secondly, the port connection is executed well: before they weren't connected and now they are and they transmit correct data between them.
> But, as a side-effect, in another component, which uses the marker data, the pose data and other parameters turn into nan (not a number). Before, they were standard initial numbers.
> So probably some parameter in that component is divided by 0 when these specific ports are connected and as a result affects the other parameters of that component.
> The problem is: I can't see which parameter is affected first.
>
> Data Flow Ports:
> In(C) KDL.Frame T_o1_o2_in <= ( use 'T_o1_o2_in.read(sample)' to read a sample from this port)
> In(C) KDL.Twist Jq_qdot <= ( use 'Jq_qdot.read(sample)' to read a sample from this port)
> Out(C) KDL.Frame T_o1_o2_out => [[nan ,nan ,nan ;
> nan ,nan ,nan ;
> nan ,nan ,nan ]
> [nan ,nan ,nan ]]
> Out(C) KDL.JntArray Chif => [nan nan nan nan nan nan ]
> Out(C) KDL.Twist JuXudot => [0 ,0 ,0 ,0 ,0 ,0 ]
> Out(C) KDL.Jacobian Jf => [nan nan nan nan nan nan
> nan nan nan nan nan nan
> nan -nan -nan -nan -nan -nan
> 0 nan nan nan nan nan
> 1 nan nan nan nan nan
> 0 nan nan nan nan nan
> ]
> Out(C) bool initialized => false
> Out(C) double Chif_distance => nan
>
> After connecting the ports, I feed the actual realtime marker position with the following code, but the nan situation already happens when the ports are connected.
> If anything, this stream command exaggerates the number of parameters that turn in nan.
>
> var ConnPolicy roscp
> roscp.transport = 3
> roscp.name_id = "/pose_marker"
> stream("marker.external_meas", roscp)
>
> Any thoughts?
It seems to me that it is not an Orocos problem, but an algorithmic one:
there is a loop closure algorithm in the VKC that assumes you have no
jumps in the state (pose).
Once you connect your port, the new data overwrites the default data you
where sending in your marker component.
The loop closure will fail and the data corrupted,
an event should be sent out to notify that the algorithm is close to a
singularity before this happens.

>
> Jon
Nick